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ABSTRACT 


The problem solved was for an autonomous mobile robot to generate a precise map of its 
orthogonal, indoor environment. The maps generated by the robot’s sensors must be perfect so they 
can be used in subsequent navigation tasks using the same sensors. 

Our approach performed map-making incrementally with a partial world data structure 
describing incomplete polygons. A striking feature of the partial world data structure was they 
consist of “real” and “inferred” edges. Basically, in each learning step, the robot’s sensors scan an 
unexplored region to obtain new “eal” and “inferred” edges by eliminating at least one “inferred” 
edge. The process continues until .:o “inferred” edges remain in the partial world. In order to make 
this algorithm possible, linear fitting of sensor input, smooth vehicle motion control, dead 
reckoning error correction, and a mapping algorithm were developed. This algorithm was 
implemented on the autonomous :uobile robot Yamabico-11. 

The results of this experiment using Yamabico-11] were threefold. (1) A smooth path tracking 


algorithm resulted in motion error of less than 2% in all exy-eriments. (2) Dead reckoning error 


correction experiments revealed small, consistent vehicle odometry errors. The maximum 


observed error was 1.93 centimeters and 1.04° over a 9.14 me.er course. (3) Precise mapping was 
demonstrated with a map accuracy in the worst case of 25 centimeters and 2° of hand measured 
maps. The ability to explore an -adoor world space while correcting dead reckoning error is a 


significant improvement over pre’-ious work [Leonard 91] [Crowley 86] [Cox 91]. 
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L INTRODUCTION 


Recent advances in computer processing speed have encouraged the development of 
increasingly capable mobile robot platforms. The popular trend in current military applica- 
tions is to accomplish the required mission with a minimum loss of life. Consequently, 
many government-sponsored efforts are underway for building systems for fighting fires, 
handling ammunition, transporting material, conducting underwater search and inspection 
operations, and other dangerous tasks now performed by humans [Everett 92]. One useful 
naval application would be a robot for inspecting tanks, voids and other dangerous spaces 
on board a military ship. This kind of robotic vehicle must first be physically robust to cope 
with the harsh ship-board environment. Additionally, this robot must have the proper sen- 
sors, mobility, and intelligence to perform a variety of tasks. This interdisciplinary set of 
problems is part of the robotics field. The capability to explore an unknown environment 
and record data about this environment is a critical capability of this robotic vehicle. This 
capability is part of a larger problem called cognition. Cognition is composed of “all pro- 
cesses by which sensory input is transformed, reduced, elaborated, stored, recovered, and 
used” [Nachtigall 86]. Automated cartography is one step towards robot cognition. 

In civilian robotics applications, robots must be able to adapt to changing circumstanc- 
es in their environment. Ideally, a useful robot should be sufficiently adaptable to function 
in a totally unfamiliar environment. A highly desirable robotic domestic application is a ro- 
bot vacuum cleaner that plugs itself into a wall socket and cleans the floors of a residence. 
This is an annoying and time consuming task for humans, and cculd instead be accom- 
plished by a robot. A robotic vacuum cleaner would be capable of entering a totally unfa- 
miliar indoor environment to clean an entire floor space with no prior knowledge of the 
floor plan. Additionally, the robot must be capable of adapting to unexpected changes in 
the house. For instance, the resident may want to rearrange the furniture for a party or add 


a new piece of furniture. The vacuum robot must be adaptable to changes in its environment 











and clean the house with no human intervention. In order to be marketed successfully, this device 
needs to be able to distinguish between a diamond earring and a crumb of food on all floor surfaces 
in the household. Why is it that such a product does not yet exist on the market? Obviously, the cost 
of such a device is prohibitively high. Additionally, the robotic technology required to perform the 
navigation and cartography tasks does not yet exist. Indoor robot navigation and cartography are the 
central issues of this dissertation. 

A robotic vehicle’s capability to explore and map an unknown work space is called automated 
cartography. There are two other tasks related to automated cartography: navigation and map repre- 


sentation. The automated cartography problem has a circular interdependence among the robot tasks, 


MAP REPRESENTATION 
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CARTOGRAPHY NAVIGATION 
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Figure 1.1 - Cartography-Map-Navigation Dependency 
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since navigation is required to perform cartography, cartography is the method of building maps, and 
the maps produced by cartography are used for navigation. Figure 1.1 illustrates this concept; each 
of the three arrows means “is required for” in this diagram. Obviously, some sort of bootstrapping 
process is required when starting without a map. In the research conducted for this dissertation, an 
autonomous mobile robot performs automated cartography starting with no map. 

The robot environment is called the world space [Hwang 92]. The robot’s internal view of the 
world space consists of some map representation. Guidance is defined as the process of controlling 
the motion of a robotic vehicle. Navigation is the process of directing the safe movement of a vehicle 
from one point to another {Dutton 78]. A robot navigation system provides commands to the guid- 


ance system for vehicle control. The guidance system uses a set of guidance rules to issue the proper 














vehicle control instructions. The three basic problems of navigation are: 1) how to deter- 


mine position (x, y) 2) how to determine direction (8), and 3) how to determine distance 
traveled (s), [Bowditch 84] [Dutton 78]. The dead reckoning processing is a common 
means of keeping track of these four parameters. Dead reckoning refers to the projection of 
a present position or anticipated future position from a previous position using known di- 
rections and distances [Dutton 78]. The term localization refers to the process of determin- 
ing a robot’s position using information from external sensors [Leonard 91]. Dead reckon- 
ing errors are normally corrected based upon localization information. Odometry is defined 
as the process of integrating the robot’s wheel motion in order to maintain an estimate of a 
robot’s current configuration. 

A configuration is a four element data structure used to describe a robot position, the 
error in a robot’s position, the position of some object and some types of path elements. The 
four elements are x, y, 8, and kK. When used to describe a robot’s position, (x,y) is the robot’s 
location in the Cartesian plane, 8 is the robot’s orientation with respect to the global x axis, 
and « is the robot’s instantaneous path curvature. Odometry estimate error is defined as the 
algebraic difference betweei. a vehicle’s estimated configuration and its actual configura- 
tion. Odometry is a purely internal means of estimating a vehicle’s position. Normally, 
wheel rotations are integrated by reading some type of wheel encoder. Odometry error 
tends to increase linearly as a function of the total distance traveled and is primarily asso- 
ciated with slip between the robot wheels and the ground. Automated odometry error cor- 
rection is defined as the process whereby an autonomous vehicle reduces its odometry es- 
timate error by determining its position with respect to some external feature in the world 
space. Features suitable for odometry correction by a given sensor are called landmarks. In 
this dissertation, only naturally occurring landmarks are used for navigation. This means 
that no artificially placed landmarks are installed in the robot’s world space to facilitate nav- 
igation. 

Map representation is the other important part of automated cartography. A map is a 


symbolic representation of some finite space; a detailed map of a small portion of the world 











may contain a large volume of information. In order to ensure that the cartography problem re- 


mains tractable, only the salient features of the world that apply to the map’s intended application 
should be represented on the map. The maps considered in this dissertation are designed specifi- 


cally for autonomous mobile robot navigation. 


A. SCOPE OF DISSERTATION 

This dissertation presents a novel software system for the automated cartography of an 
unknown world space by an autonomous mobile robot. The automated cartography algorithm 
developed in this dissertation is an efficient means for an autonomous mobile robot to effectively 
explore an unknown world space while building a spatially consistent map of the space. The FSM 
problem domain has three major aspects: automated workspace navigation and exploration, path 
tracking as the method of vehicle control, and automated landmark recognition with odometry 
error correction. Previously, no approach has successfully integrated the solutions of these three 
component problems into a single software system. 

How is the automated cartography algorithm different and better than the rest? As already 
explained, the map representation, the vehicle navigation and the cartography requirements are 
intimately related. This algorithm allows the robot to use a partially built map to decide which areas 
of the world space require exploration. The algorithm uses office-building heuristics to take 
advantage of features found in most office buildings. An example of this heuristic is that an indoor 
hallway is assumed to have straight walls broken by doorways. Basically, a scan model derived 
from a single robot motion is merged with the robot’s partial world model (PW) and the state of the 
new PW is used to guide the search for unexplored portions of the world space. The path tracking 
vehicle control sub-system provides smooth vehicle control and the necessary vehicle motion for 


odometry error determination and correction while the vehicle is moving. 
B. THE AUTOMATED CARTOGRAPHY PROBLEM 


1. Problem Statement 


The automated cartography problem involves the incremental modeling of an unknown, 


indoor world space. Automated cartography involves a robot system R mapping a planar, static, 
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people-free, indoor world space W. All objects in the environment are represented as rigid, 
convex polygons in the robot’s world space. All areas mapped lie in the same Cartesian 
plane. This enables R to map, for example, a single floor of an office building. R builds a 
large scale, spatially-consistent, metrically-accurate map PW after being placed in any 
arbitrary configuration C, in the world space W. The robot sensors S are 12 fixed ultrasonic 
range finders. R starts with no prior knowledge of W and it must navigate in a manner 
required to explore the entire accessible portions of W using its partially built map PW and 
sensors S. Spatial consistency of PW is maintained by the robot’s capability to update its 


dead reckoning configuration C using naturally occurring landmarks L; in the work space. 


2. Example of a Typical Automated Cartography Experiment 

The enclosed-space experiment illustrates the problem best: an uncluttered, 
enclosed, indoor space in a typical office building is selected as the robot’s world space. 
The automated cartography software is loaded onto the robot and the robot is placed in any 
arbitrary starting configuration with no map of the workspace loaded into its memory. The 
program is started and the robot is left in the enclosed space. The robot spends about one 
hour exploring the entire enclosed space, building and refining a precise map of all 
accessible regions in this enclosed space. Upon map completion, the robot returns to its 
Starting position ready to execute commanded trajectories using its learned map for 


accurate position determination at arbitrary locations in the environment [Leonard 92]. 


3. Assumptions 
The 2D assumption - The world space is a flat planar world with obstacles. The 
floor is the x, y plane. All obstacles faces are perpendicular to the x, y plane and have a 
constant size along the positive z-axis. This assumption is required to assure a good sensor 
return from all objects. 
Orthogonal Wall Assumption - Walls in the robot’s world space are always 


rectilinear, as are found in most office buildings. 





Rigid Body Assumption - The vehicle and all objects in the robot’s world space are 
rigid. The surface of any object in the world space may be represented by a single configuration. 
Static Worid Assumption - All objects in the world space are immobile both in a relative 


and an absolute sense. 


C. ORGANIZATION OF DISSERTATION 


Chapter II reviews the major challenges and issues currently faced in the field of mobile robot 


cartography. The issues discussed represent the major hurdles that must be overcome for automat- 
ed cartography. This chapter also describes research work in the field of mobile robot navigation. 
The significant contributions in the field are reviewed. 

Chapter III gives a detailed description of the Naval Postgraduate School autonomous mobile 
robot Yamabico-11 vehicle and the simulator used to develop the robot software used in this dis- 
sertation. The limitations of ultrasonic sonar sensors for robotic applications are described. This 
simulator description provides an introduction to the Model-based Mobile robot Language (MML) 
programming environment. The simulator proves to be an important software development tool 
since some debugging is tedious or even impossible on the robot. Efforts to improve software ef- 
ficiency, organization, and functionality are tried in simulation first before testing begins on the 
robot. 

Chapter IV presents the software architecture for the vehicle control for Yamabico-11, the ro- 
bot test-bed for this dissertation. The software scheduling system, a geometry module, the sonar 
subsystem, and the input/output subsystem are described in detail. 

Chapter V presents the theory of robot odometry correction. An algebraic approach is taken 
to describe a robot configuration, the error in the robot’s configuration as well as the configuration 
of landmarks in the robot’s world space. A detailed explanation of the method of real-time, on-line 
odometry correction is provided. 

Chapter VI provides the basic data structures for world representation. Chapter VII describes 
a theory for robot automated cartography using an idealized sensor. This provides the theoretical 





foundations for robot cartography using real sensors. The limitations of the real sensors im- 
pose the modifications required to the ideal algorithm presented in Chapter VII. 

Chapter VIII describes the theory of rea] automated robot cartography using the auto- 
mated cartography algorithm. Each aspect of this algorithm is explained using examples. 
The means for on board map representation, robot exploration and map refinement are de- 
scribed. 

Chapter IX gives the details of the experiments performed and the results obtained. 
Experimental results are plotted to help the user better review the results. Chapter X of the 
dissertation finishes with a summary of the conclusions drawn from the theory and the ex- 
perimental results and recommendations for further research. 

Appendix A is a comprehensive robot user’s manual. Appendix B provides MML lo- 
comotion source code. Appendix C gives the sonar functions used for feature extraction. 
Appendix D lists the odometry correction source code and appendix E gives the real robot 
cartography source code. 








I AUTOMATED CARTOGRAPHY: MAJOR CHALLENGES AND ISSUES 


This chapter is designed to provide the reader with background on the major challeng- 
es and issues in robot cartography. Automated cartography represents a significant research 
undertaking in the development of an intelligent autonomous robot capable of exploring its 
environment. This chapter addresses the major challenges and issues that were addressed 
in the development of the Yamabico-11 automated cartography system. They are presented 
briefly in this chapter to lay a firm foundation for understanding and to provide an overview 
of the design decisions that went into Yamabico’s software. This chapter cites significant 
robotics projects from the literature to illustrate each issue. 

Map representation is a critical issue because the world is rich with features and a ro- 
bot’s memory is typically of limited size. Therefore, only the important features should be 
stored since the map must contain the necessary information for robot navigation but can- 
not be too large. Robot motion planning is an important challenge since a robot must plan 
a purposeful route through the world space in order to map the space. Software architecture 
of the entire control system is important since this factor determines the software’s efficien- 
cy and modifiability to a large extent. Some software architectures lend themselves to car- 
tography more readily than others. Robot sensing is a critical issue with regard to cartogra- 
phy because a robot must sense its world in order to build a map of it. Robot localization 
and navigation are important challenges because the robot must navigate effectively to ex- 
plore its environment. Dead reckoning errors must be corrected by means of localization in 
order for a robot to build a spatially consistent map. Robot exploration is necessary in order 
for the robot to move its sensors to all reachable portions of the world space. These issues 
are not limited to Yamabico; they span the fields of robotics, navigation, computer science 


and mathematics. 





A. METHODS FOR MAP REPRESENTATIONS 

A robot must have a model of objects in its environment before it can plan a collision- 
free path through its world space. The robot may have an a priori map of its environment 
or it may use sensors to acquire knowledge about its surroundings. Sensors are typically 
used to build a depth map of the surrounding environment. A depth map is a statical repre- 
sentation of many range finder returns. This information is normally converted into a some 
compact representation to save memory space and to speed up computations. Once infor- 
mation about shapes and configurations of objects is acquired, it can be represented in sev- 
eral ways. The trade-offs between simplicity, resolution, and computational efficiency must 
be carefully considered when choosing the best means of representation for a specific ap- 
plication. The remainder of this section reviews the commonly used map representation 


techniques. 


1. Grid Representation 

The grid representation method divides the robot’s environment into an array of 
identical cells. These cells are typically rectilinear. The robot’s environment is represented 
by marking the individual cells as either one if it is occupied by an object or as a zero for 
unoccupied. The simplicity of this method has many computational advantages, especially 
on a massively parallel computer. The cell size governs the overall resolution in the robot’s 
environment; smaller cells give higher resolution but incur a penalty in terms of on board 
storage requirements and computational efficiency. Elfes called this factor the resolution 
axis [Elfes 87]. Moravec used certainty grids for mobile robot map representation 
[Moravec 87]. Borenstein and Koren used a grid-type representation called a vector field 
histogram on the robot Carmel at th: University of Michigan [Borenstein 92]. Beckerman 
and Oblow [Beckerman 90], Everett [Everett 89], Noborio et. al. [Noborio 90] and Zelinsky 
[Zelinsky 88] have also used various grid-based representations to build maps from sonar 
data. 


Elfes implemented an stigatimnous motile robot navigation system called Dolphin on 
the Neptune (indoor) and Terregator (outdoor) mobile robots. This system used sonar range 
data to build a multilevel description of the robot’s surroundings using a grid-based map 
representation called occupancy grids [Elfes 87]. Elfes used a multilevel description of the 
robot’s operating environment. Several dimensions of the representation were defined: the 
abstraction axis, the geographical axis, and the resolution axis. The system was completely 
autonomous in that it had no a priori model or knowledge of its surroundings [Elfes 87]. 
Range measurements from multiple points of view were combined into a sonar map while 
accounting for uncertainties and errors in the data. By combining the evidence from many 
readings as the robot moved in its environment, the area known to be empty was expanded 
{Elfes 87]. Elfes used 24 ultrasonic range finding transducers arranged in a circular array 
to build dense two dimensional maps based upon empty and occupied volumes in a cone in 
front of the sensor. 

Elfes’s research involved grid-based mapping, whereas this dissertation focuses on 


feature-based mapping. The reason the feature-based approach was chosen is that the com- 


putational complexity of this approach is lower than the o(n'y complexity of Elfes’s work 
where n is the number of grid squares in the map. In the Dolphin system, all map compu- 
tation was done off-line on a VAX-11/780. Yamabico’s mapping system does all mapping 
computation using the on board processor. On board processing eliminates communication 
delays between the processor and the robot, and allows Yamabico full autonomy. The Dol- 
phin system used Polaroid laboratory grade ultrasonic range transducers with a 30 degree 
beam width. The 3 dB beam width was approximately 15 degrees. Yamabico uses a colli- 
mated beam sonar sensor with separate emitter and receiver. 

Grid-based approaches to mapping make weaker assumptions about the environ- 
ment than the polyhedral approach since grid type representations do not explicitly repre- 
sent surface boundaries in the robot’s world space [Leonard 91]. Thus arbitrary inaccura- 


cies and uncertainties are always present in grid-based approaches. 
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2. Cell Tree Representation 


The cell tree representation is also called quadtree for two dimensional (2D) rep- 
resentations or octree for three dimensional (3D) representations. This method was devel- 
oped to improve the overall efficiency of the grid method when representing a large object 
or a large open space. The cell tree representation divides the robot’s world space into a 
small number of large cells. The cells are not necessarily all the same size. Cells completely 
inside or outside of the objects are marked either occupied or empty. Cells partially occu- 
pied are further divided. This process is repeated until the cell size reaches an arbitrary res- 
olution limit. This method represents a significant reduction in storage space requirements 
at the expense of additional complexity. Several researchers have used cell trees for map 
representation [Fryxell 88] [Airey 90]. The cell tree approach is a stronger approach to 
mapping than the grid-based approach, but still does not explicitly represent object surface 
boundaries as does the polyhedral approach. Cell tree representation straddles the represen- 
tation spectrum midway between grid-based and polyhedral representations since the divi- 


sion of space is defined to some extent by the objects being represented. 


3. Polyhedral Representation 

A polyhedron is a solid figure having many faces. Objects in the robot’s environ- 
ment may be approximated by the unions of polyhedra. This is an efficient means for rep- 
resenting a robot’s world space since much less storage space is used than for the grid- 
based method. Only the boundaries between open space and objects are represented, in- 
stead of every grid-square in the world space. Curved surfaces must be approximated as 
planar surfaces to maintain the polyhedral representation. Efficient (O(nlogn)) algorithms 
exist for computing the intersection of and the distance between two polyhedra [Hwang 
92]. A robot’s world space was first represented by polygonal objects by Lozano-Perez in 
his influential Configuration Space (C-Space) [Lozano-Perez 79]. 
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4. Constructive Solid Geometry Representation 
The Constructive Solid Geometry (CGS) method of free space representation is 
used in solid modelers. The CGS represents objects as unions, intersections, and set differ- 
ences of primitive shapes including spheres. This method has the advantage that curved sur- 
faces can be represented with a small number of parameters specifying the curve. This 
method is often used in conjunction with computer aided design (CAD) systems for envi- 


ronment mapping [Hwang 92]. 


5. Topological Map Representation 

The topological map representation approach uses a graph-theory approach to 
represent robot free space. A graph G = (V,E) is a finite non empty set V of elements called 
vertices, together with a set E of two-element subsets of V called edges [Gould 88]. Vertices 
represent places the robot may visit and edges represent pathways used to travel between 
nodes. The robot Huey at Brown University also used a topological graph to represent the 
map in the 1992 AAAI robot contest [Davis 93]. 

Mataric used a topological map representation with the robot Toto [Mataric 92]. 
Toto navigated using ultrasonic range finders and a flux-gate compass. The experiment in- 
cluded automated building of simple topological maps of the robot’s world space. Land- 
marks were represented as a tuple <7, C, L, P> where T was the landmark type, C was the 
average compass bearing, L was the landmark’s length and P = (x, y) was a course position 
estimate. Whenever a landmark was detected, it was matched to all known landmarks that 
were stored in a graph structure. Either a unique match or no match occurred. Localization 
was a simple process of comparing the stored landmark descriptor <Z, c, /, p> with the robot 
current sensory information </’ ,c’ ,’ p’>. The map structure consisted of a graph with each 
node representing a robot-detected landmark. Edges defined the connections between the 
landmarks. 

The work described in this dissertation is different in several ways. No flux gate 


compass is used for determination of Yamabico’s orientation. In both cases, landmarks are 
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automatically recognized and used for odometry correction. In Yamabico’s case, the posi- 
tion as well as the orientation of individual landmarks is used to correct the robot’s orien- 
tation. This was not true for Mataric’s work. Mataric’s algorithm recognized landmarks 
based upon their length and position whereas the automated cartography algorithm recog- 
nizes landmarks by their precise position and orientation. Finally, Mataric’s algorithm was 
essentially a modified wall follower behavior; it did not adapt well to open spaces. The au- 
tomated cartography on the other hand greedily acquires and maps the open space available 
and is not restricted to modified wall following. 

Topological representations provide a compact method of storing a map with many 
features. However, topological maps do not explicitly record the metric distance between 
vertices. This renders topological maps less useful for robot navigation than maps with dis- 


tances explicitly represented. 


B. APPROACHES TO ROBOT MOTION PLANNING 

Robot motion planning is the process whereby a robot’s path is planned based upon 
the robot’s current configuration and the representation of the robot’s environment. Plan- 
ners use a world model as an input to plan a safe, efficient path from one configuration to 
another. Not all robotic systems plan the robot’s motion in a deliberative fashion. In fact, 
there exists a broad spectrum of motion planners, from no plan/no model to a flexible plan 
to a rigid, unalterable plan. Many different methods have been developed for robot motion 
planning. Some methods are widely applicable, whereas others solve only a narrow range 
of motion planning problems. 

The motion planning problem is defined as follows. Let R be a robot system having k 
degrees of freedom, and suppose that R is free to move in a two or three dimensional space 
V amidst a collection of non-moving obstacles whose geometry is known to the robot sys- 
tem. The mution planning problem for R is: given an initial position Z, and a desired final 
position Zz, determine whether there exist a continuous obstacle-avoiding motion of R from 


Z; to Z2 and if so plan such a motion [Schwartz 88}. The general motion planning problem 





can be solved in polynomial time in the number n of algebraic constraints defining FP 
where FP denotes the space of free positions [Schwartz 83]. 

Motion planning methods fall into four general categories: skeleton, cell decomposi- 
tion, potential field, and mathematical programming [Hwang 92]. Most motion planning 
problems can be approached by one of these four methods. Hybrid combinations of these 


approaches are often used in developing new motion planners. 


1. Skeleton 

In the skeleton approach to motion planning, the set of all feasible motions is 
mapped onto a network of one dimensional lines [Hwang 92]. These lines represent safe 
pathways for robot motion in the free space. This approach has also been called the retrac- 
tion, roadmap, or highway approach. The advantage of this method is that the search for a 
solution is limited to the skeleton. Using this approach, motion planning is accomplished 
by first moving the robot from its starting position to a point on the skeleton. Next, the robot 
is moved from the goal configuration to a point on the skeleton. Finally, the two points on 
the skeleton are connected using lines in the skeleton. Two well-known skeletons are the 
visibility graph and the Voronoi diagram [Canny 88]. One advantage of this method is that 
skeletons for a large area can be preprocessed using a known world model as input. Brooks 
represented free space as a union of possibly overlapping generalized cones. A generalized 
cone has an axis of a certain length and a boundary on each side of the axis. He used gen- 
eralized cones to represent free space in a 2D world and the robot traveled on spines of the 
generalized cones [Brooks 83]. An improved quality path was obtained by representing free 


space as a union of generalized cones and convex polygons [Kuan 85]. 


2. Cell Decomposition 
In the cell decomposition approach, the free configuration space is first decom- 
posed into a set of simple cells and then adjacency relationships between the cells are com- 
puted. To find a collision free path, the cells containing the start and goal configurations are 


connected with a sequence of empty, adjacent cells. In this method cell boundaries can be 
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object dependent or independent. With an object dependent decomposition, boundaries of 
obstacles are used to generate cell boundaries. The free space is the union of the free cells. 
With this method the number of cells is smal} but the complexity of the decomposition is 
high. 

With object-independent decomposition, the configuration space is partitioned 
into cells of a simple shape, then each cell is tested for occupancy. Since the cell shape and 
location are independent of the object shape and location, the cell boundaries do not tightly 
enclose the object [Hwang 92]. Increasing the number of cells can make the representation 
error arbitrarily small. Examples of object-independent cell decompositions are grid and 


quadtree. 


3. Potential Field 

The potential field approach treats the robot as a particle under the influence of an 
artificial potential field whose local variations are expected to reflect the “structure” of the 
free space [Latombe 91].This approach has been compared to a sticky marble rolling down- 
hill on the interior surface of a bowl [Arkin 89]. The goal point for the robot is the lowest 
point in the bow] and obstacles are represented by inward dents in the bowl. This approach 
constructs a scalar function called the potential that has a minimum when the robot is at the 
goal configuration, and a high value on obstacles in the configuration space. At all other 
locations in the configuration space the function is sloping downward toward the goal con- 
figuration. The robot moves toward the goal by following the negative gradient of the po- 
tential to the minimum. 

To use this approach, an obstacle potential is constructed. This field has a high 
value on the obstacles and decreases monotonically as the distance from the obstacles in- 
creases. Superimposed onto the obstacle potential is a goal potential that has a large nega- 
tive value at the goal and increases monotonically as the distance from the goal increases. 


This approach has the advantage of being simple but there are usually several lo- 


cal minima other than the goal. These minima can trap the robot. Another disadvantage is 





the potential field expression becomes very complex when there are many concave obsta- 
cles in the configuration space. The potential field approach is best used as a local motion 
planning algorithm in conjuction with some other global motion planning algorithm 
(Hwang 92]. Most planning methods based on the potential field approach have empirical 
connections. They usually do not guarantee that a path will be found even when one exists. 
They are, however, particularly fast in a wide range of situations. Potential field planners 
are increasing popular because an efficient and reliable motion planner can be constructed 


using this paradigm [Latombe 91]. 


4. Mathematical Programming 
In this approach, motion planning is formulated as a mathematical optimization 
problem that finds a path between the start and the goal configuration by minimizing some 
scalar quantity. Mathematical programming has difficulties with non-unique solutions, sin- 


gular matrices, and non-static environments. 


C. SOFTWARE ARCHITECTURE 

The software architecture is the structure of the contro} system for a robotic platform. 
Typically, a robotic vehicle and its associated control software have been developed in tan- 
dem [Busnel 79). The robot’s morphology and its software architecture are closely related 
since software and hardware are both designed to solve some particular problem [Brooks 
93}. The various approaches to system architecture for autonomous vehicle control have 
been grouped into six categories [Arkin 89]. These classes are monolithic control, hierar- 
chical (deliberative) control, behavior-based control, hybrid systems, distributed control, 


and machine learning systems. 


1. Monolithic Control 


Monolithic control systems are limited capability systems typically used on a fac- 
tory floor. These systems tend to be sensor dependent and employ a teaching pendant ap- 


proach to motion control. The robot’s environment is engineered so that the robot uses ar- 


16 








tificial landmarks for localization and navigation. These systems are inflexible and not gen- 


eralizable. However they are advantageous since they are easy to develop. 


2. Hierarchical (Deliberative) Control 

Hierarchical systems typically have a top-down control structure. The complexity 
of the system is managed by abstracting complex vehicle behavior into successively less 
complex functional levels in the same manner as structured computer programming. Typ- 
ically, the high level planner is at the top of the hierarchy and the low level servo control is 
at the bottom. These systems normally maintain a symbolic world model to support sensory 
processing. The world model contains the robot’s current state and the current state of the 
robot’s environment. 

Commands are passed from the top level symbolic planner down the hierarchy. 
Sensory information is passed up the hierarchy. The update rate of a given level tends to 
increase as one moves down the hierarchy. The planning horizon for each level tends to 
grow longer as one moves up the hierarchy. The state space reasoning tends to occur at a 
high level and tends to be purely symbolic in nature. These systems are characterized by a 
slow response time to sensor input. This can be a significant disadvantage in a rapidly 
changing environment. The symbolic reasoning gives the advantage of a high-level, global 
intelligence due to deliberative reasoning. These system: are also characterized by a vari- 
able latency due to the running time of different deliberative portions of the system. The 
Hughes control system for the Autonomous Land Vehicle (ALV) is a classic example of a 


hierarchical control system [Daily 88]. 


3. Behavior-Based (Reactive) Control 
Autonomous vehicle control using the behavior-based control architecture tends 
to focus on reaction to input stimuli rather than deliberative planning. These systems are 
typically built in layers of successively more complex behaviors. The lowest level of be- 


havioral competence for the robot is designed, built, and tested first. Progressively higher 
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level layers of behavioral competence are then added. These layers all run independently 
{Brooks 86a]. 

This control method employs no centralized intelligence. Instead the intelligence 
is distributed among the layers of competencies. Each layer processes sensory data and out- 
puts a specific behavior. These behaviors compete through a network of surpression nodes. 
The vehicle’s overall behavior is said to “emerge” from the interaction of multiple, com- 
peting, unintelligent layers. 

Reactive control is characterized by no central world model since the world pro- 
vides its own model [Brooks 91]. Therefore these systems tend to be representation-free. 
Since the individual layers have short sensor-effector arcs, these systems have the advan- 
tage of real-time response to input stimuli. No central intelligence system is operating, 
therefore these systems tend to have low-level overall intelligence when compared to de- 
liberative systems. The layers are typically simple, consequently, these systems tend to ex- 
ecute only simple computations. Brooks at MIT coined the term subsumption to mean high- 
er level robot behaviors subsume lower level behaviors when appropriate. He rejected tra- 
ditional Artificial Intelligence (AI) as dogma and ridiculed precise robot navigation 
research. He emphasized robotics systems with an ongoing physical interaction with the en- 
vironment [Brooks 90]. He believed the world provided the robot with the best model. 
Based on evidence from evolution, he believed robot mobility, acute vision and the ability 
to Carry out survival related tasks in a dynamic environment provided the basis for the de- 
velopment of intelligence [Brooks 91]. Further, he argued that issues of representation 
stalled artificial intelligence research. More surprisingly, he claimed that traditional repre- 
sentation was unnecessary. His robotic systems architecture was decomposed into indepen- 
dent and parallel producers that interfaced directly with the world through perception and 
action. Brooks adopted a layered architecture approach and built completely autonomous 


mobile agents that coexisted in the world with humans and called them Creatures [Brooks 


91). These Creatures were expected to cope with changes in their world. They were robust 








and adaptable to changes in the envirozment, maintained multiple goals, and were able to 
capitalize on opportunities presented by the environment. 

Brooks argued that no central symbolic information processor was necessary to 
build the Creatures. Instead, the robot’s software was built incrementally by the use of be- 
havioral layers. Each of these layers added an additional behavioral competence. Brooks 
called the approach subsumption architecture since the layers acted independently and in 
parallel with other existing layers. The system had no centralized control, and no central- 
ized repository for sensor information. Further, Brooks claimed that intelligent robot be- 
havior emerged even though the robot stored no internal representation of the physical 
world. Maintaining no internal representation of the world has some significant limitations. 
For instance, a robot tasked with repeatedly traversing the same obstacle field would great- 
ly benefit from an internal representation since a path could be planned around a previously 
encountered obstacle. Instead a robot with no memory of obstacle location is doomed to 
repeat the same obstacle avoidance behavior each trip. One researcher expressed his skep- 
ticism by saying “subsumption architecture is better suited to building thermostats than in- 
telligent agents” [Wallich 91]. This means that researchers who do agree with Brook’s ap- 
proach do not believe that useful, intelligent behavior will ever emerge from a collection of 
primitive reflexes. 

As one of Brook’s students, Connell demonstrated a subsumption program for 
gathering soda cans in an office building environment using the robot Herbert. The robot 
built no maps of its surroundings, but managed to wander about, find and pick up a soda 
can and return to its starting position [Brooks 93]. This experiment required careful place- 
ment of the robot and the soda can since the robot followed only one path in response to 
external stimuli. Brook’s philosophy represented the opposite end of the representation 
spectrum with respect to the Yamabico project. Yamabico’s software system relies heavily 
on centralized contro!, <xplicit goals, and an internal representation of the world. Brook’s 


software was compose “if individual, competing behaviors to produce emergent intelligent 
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behavior. Brooks has rejected any sort of centralized representation on the extemal envi- 
ronment whereas Yamabico builds a detailed map of its world. 


4. Hybrid Systems 

Hybrid vehicle control architectures lie on the continuum between the hierarchi- 
cal and behaviorist extremes [Bymes 93]. Hybrid architectures attempt to combine the best 
characteristics of both hierarchical and behavorist architectures. The explicit global intelli- 
gence advantage of hierarchical systems is typically combined with the quick-reacting, re- 
flexive behavior of the behavior-based models. Plan formulation in hybrid systems tends to 
borrow from hierarchical systems in order to gain deliberative intelligence. Plan execution, 
however, is similar to reactive control. 

The Autonomous Land Vehicle (ALV) was designed and developed by Martin 
Marietta Aerospace as a test bed for research in autonomous mobility systems [Turk 88]. 
Its dimensions were 2.7 meters wide, 4.2 meters long, and 3.1 meters high, and provided 
the capacity to carry all power, sensors, and computer systems necessary to support auton- 
omous operations. The ALV weighed approximately 16,000 pounds fully loaded yet was 
capable of traveling both on and off road. The vehicle had an eight-wheel drive, was diesel 
powered, and driven by hydrostatic transmission. A wide range of sensors was employed, 
and included a video camera, a laser range finder, and wheel-mounted odometers. 

A control software architecture was developed for the ALV by Hughes Artificial 
Intelligence Center [Daily 88]. The hybrid architecture was organized into four levels and 
each level contained planning and perception functions. At the highest level, the mission 
planner was used to define mission goals and constraints. These were passed to the next lev- 
el, which maintained the world model and developed plans based on stored maps. The oe 
sulting route plan was then passed to the third level containing the local planner. The local 
planning module selected and monitored reflexive behaviors at the lowest level. It was at 
that level that reflexive behaviors were used as real-time operating primitives [Payton 90]. 


Reflexive behaviors were independent of each other and executed concurrently, however, 
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it was the responsibility of the local planner to partition the appropriate behaviors depend- 
ing on the current environment. The ALV architecture was field tested and was the first sys- 
tem to demonstrate obstacle avoidance in natural terrain [Olin 91]. 


5. Distributed Control (Blackboard Paradigm) 

The term blackboard has been applied to any globally-accessible data structure to 
which multiple processes may communicate by posting messages. The blackboard method 
of vehicle control is characterized by one or more global data structures called blackboards. 
These blackboards constitute the working memory (or global database) for the control sys- 
tem. Separate knowledge sources read from and write to the blackboards. These knowledge 
sources reason about information obtained from the blackboard and write their conclusions 
back to the blackboard. These systems are typically synchronous and cooperative in oper- 
ation. The Task Control Architecture deployed on the Ambler walking robot at Camegie- 
Mellon University (CMU) is a distributed control system [Simmons 91] [Simmons 92]. 


6. Machine Learning Systems 

Artificial neural nets are computer programs designed to imitate the brain’s abil- 
ity to learn from experience. A common type is the feed-forward neural net. Machine learn- 
ing is accomplished by encoding information in the net’s simulated synaptic connections. 
Neural nets have been used as an architectural approach for robot control [Nehmezow 92] 
(Thrun 92]. 

The CMU robot Odysseus placed fourth in the American Association for Artifi- 
cial Intelligence (AAAI) robot competition in 1992 [Davis 93]. Odysseus used ultrasonic 
sonar data to build environmental maps off-line at CMU Artificial Intelligence (AI) Lab. 
While at CMU, Thrun developed a system to explore and model an office building envi- 
ronment efficiently [Thrun 93]. He used the robot Columbus, a modified Heathkit robot. 
This was an autonomous, wheeled robot with bumper sensors, a rotating sonar sensor, and 
a motion sensor for odometry. The exploration system used an instance-based learning 


technique for developing the map. Two artificial neural networks were used to encode the 
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characteristics of the robot’s sensors and the characteristics of a typical indoor environment 
{Thrun 93]. One neural network encoded sensor interpretation and the other encoded con- 
fidence assessment. Exploration was achieved by navigating the robot to regions with little 
sonar data [Thrun 92}. The world model was represented using a grid-based mapping tech- 
nique with four inch grid squares. 

Neural network training allowed the network to encode the specific characteris- 
tics of the sensors as well as those of typical environments of a mobile robot; it captured 
knowledge independent of any particular environment the robot might face. An instance- 
based approximation technique was employed for modeling the environment. Exploration 
was guided by an anytime planner based upon dynamic programming for planning low-cost 
paths to poorly explored areas [Thrun 93]. The approach to model building and position 
control was successfully used as part of the CMU entry Odysseus in the AAAI robot com- 
petition in 1992 [Thrun 93}[Davis 93}. 

A neural network approach to automated cartography was demonstrated by Neh- 
mzow [{Nehmezow 92]. The robot Alder used a self-organizing network to construct inter- 
nal representations of the world it experienced as it moved around. The resulting neural net- 
work was a map, but a map in motor-sensory space rather than the physical space [Nehm- 
ezow 92]. In this way robot behavior and sensing were well coupled to the environment and 
the task of map building. 

Neural networks are limited for robot cartography applications because they are 
typically not portable among robot platforms. Neural network programs tend to be opaque 
with respect to human understanding and do not scale well to larger software systems. Ad- 
ditionally, the long training time for a neural network system is a distinct disadvantage in 


robot navigation applications. 


D. ROBOT SENSING 


Although sensor interpretation and world modeling are fundamental for robots to op- 


erate in the real world, robotic perception is still one of the weakest components of current 
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robotic systems [Iyengar 91]. The advantages and disadvantages of the most commonly 


used types of robot sensors are presented to help the reader understand sensor interpretation 
issues, sensor integration and the recovery of the world model from the robot’s spatial per- 
ception. The main problems in robotics perception are interpretation of noisy sensor data, 


computational overhead required to process sensor input, and sensor integration. 


1. Laser and Light Range Finders 

There are two basic laser range finder designs dependent upon the round trip time 
of flight to objects in the environment. The first kind measures phase shift in a continuous 
wave modulated laser beam that leaves the source and returns to the detector coaxially. The 
second measures the time a laser pulse takes to go from the source, bounce off a target point 
and return coaxially to a detector. Since light travels at approximately one foot per nano- 
second, the supporting instrumentation must be capable of 50 picosecond time resolution 
for a range accuracy of plus or minus one quarter inch [Jarvis 93]. 

The ALV used a laser range scanner for navigation during off-road operation. The 
laser range scanner was an effective sensor in this type of environment. In a structured, in- 
door environment a smaller, more maneuverable robot is required to perform the mapping 
task. One important indoor limitation is doorway width. The large size of the laser range 
finder used on the ALV prohibited its use for indoor applications. Also, since most indoor 
spaces have flat floors, a 3D terrain mapping system is unnecessary. 

Laser range finders provide 3D data directly by active sensing. At CMU, the Au- 
tonomous Land Vehicle and Planetary Exploration projects focused on perception of out- 
door terrain for path planning and object recognition [Hubert 88]. Perception techniques for 
mobile robots have been validated by using real robots in real environments. 3D vision 
techniques have been implemented on three mobile robots developed by the Field Robotics 
Center at CMU: the Terregator, the NavLab, and the Ambler. 

The Terregator was a six-wheeled vehicle designed for rugged terrain. The Nav- 


lab, with all computing equipment on board, was a converted van designed for navigation 
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on roads or on mild terrains. The Ambler was a hexapod walking robot designed for study- 
ing robotic exploration of Mars and was capable of traversing steep slopes, rocks, and wide 
gullies. 

The active exploration of other planets by mobile robots demands that they be ful- 
ly autonomous. A manned mission, even to Mars, is highly unlikely in the foreseeable fu- 
ture. In addition, conventional teleoperation of robots appears to be impractical due to the 
long time delays in signal transmission (up to 30 minutes) over the extreme distances in- 
volved. An alternative solution would involve an autonomous mobile robot capable of safe- 
ly navigating extremely rugged terrain while intelligently gathering materials and telemetry 
readings and returning them to earth for analysis. The National Aeronautics and Space Ad- 
ministration Jet Propulsion Laboratory (NASA/JPL) Mars Rover uses four laser sensors for 
object recognition. 

CMU used a time-of-flight laser range finder developed by the Environmental 
Research Institute of Michigan (ERIM). This was a phase difference type device. A two- 
mirror scanning system allowed the beam to be directed anywhere within a 30 degree by 
80 degree field of view. The ERIM sensor gave 64 by 256 range images coded on eight bits 
from zero to 64 feet with a range resolution of three inches. 

The ALV used a laser range scanner to measure the distance along the line of sight 
to the nearest object. A phase-shift laser scanner was used with a maximum range of 64 feet 
and a range resolution of 1% (3 inches). A Cartesian Elevation Map (CEM) was built to 
represent laser range data. The CEM was a downward-looking terrain representation that 
was used for autonomous navigation [Olin 91). Multiple CEMs were fused together to build 
traversibility maps that were based upon artificially moving a model of the ALV over the 
a model of the sensed terrain. A map-based planner used digital terrain data to determine 
the vehicle’s route which was represented as a set of subgoal point locations. 

The HILARE multi-sensory system included an array of 14 ultrasonic emitter-re- 
ceivers with a maximum range of two meters. A camera and laser range finder mounted on 


a pan and tilt platform provided the robot with 3D data about the environment. HILARE 
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also had an infrared triangulation system used in areas where fixed navigational beacons 
were installed. Robot localization was performed by reference to fixed infrared beacons 


when available. Sonar and laser fixed obstacle edge referencing were used when no bea- 
cons were available. HILARE explored its environment and performed automated cartog- 
raphy by a space structuring method that divided the known world into simple polygonal 
shapes. HILARE then moved in a fashion that expanded its known world and built a map 
using laser range finder data. 

Using active sensing such as a laser range finder has the advantage of eliminating 
the calibration problems and computational cost inherent in passive techniques such as ste- 
reo vision [Hubert 88]. A second advantage is the lack of sensitivity to outside illumination 
conditions, which considerably simplifies the image analysis problem. This is particularly 
important for outdoor navigation since scene illumination varies widely. Other advantages 
of laser methods include high data rate, accuracy, and long range. 

Current disadvantages include expensive equipment cost for laser sensors. Addi- 
tionally, in some cases lasers are color dependent, such that shiny surfaces give either no 


range due to poor reflectance or false range values. 


2. Infrared Range Finders 


Infrared sensors are active emitters sensors that work on a send/receive format. 
Infrared range finders use light with a frequency just below the visible spectrum. The sen- 
sor emits an infrared light from one source and measures the amount of reflected light with 
one or more light detectors. Since these devices measure light attenuation, they are highly 
biased by the environment. Object color, object orientation, and ambient light all contribute 
to erroneous readings. Since the transmission signal is light instead of sound, these sensors 
can have a high sampling rate. Due to noise factors, infrared range measurements are only 
useful for short distances [Crowley 89]. Infrared sensors are frequently used to provide 


range data inside the minimum sonar range (typically 17 inches) for Polaroid sensors. 
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Infrared sensors have the advantage of being small and low cost. The Khepera™ 
miniature robot at the Laboratorie de Microinformatique in Switzerland is the size of a soda 
can but has 37 infrared range finders as its primary sensor [Modada 93}. Therefore, they are 
employed on many smaller robotic platforms. Infrared sensors have the disadvantage that 
their output is not proportional to the target range since they are adversely affected by am- 
bient light conditions and by the color and texture of the target’s surface. Infrared sensors 
have a relatively short maximum range, normally about 20 centimeters. This limits the sen- 


sor to certain “close-in” applications. 


3. Contact Sensing 

Contact sensing includes force and tactile sensing methods. Force sensing mea- 
sures the resultant mechanical effects of contact, while tactile sensing involves the detec- 
tion of a wide range of local parameters (physical and chemical) affected by contact [Dario 
86]. Contact sensing limits a robot’s ability to quickly gather data about its environment 
since some part of the robot must physically contact the environment. Contact sensing also 
has limited resolution and is limited by the robot’s range of motion. 

Tactile sensing has been ignored historically in favor of other types of sensing, 
particularly vision. Tactile sensing is important for short range recognition tasks, assembly 
and parts-fitting work and inspection tasks. Robotic tasks that call for close tolerances or 
low absolute error can benefit from tactile sensor input. 

Some examples of contact sensors on mobile robots are bumpers, whiskers, and 
feelers. These are simple force sensors that employ some type of contact switch that shuts 


when contact is made. More complex tactile sensors measure feedback force and are often 


found on robotic arms, hands, or fingers. The Genghis™ robot designed by Brooks at MIT 
uses force feedback on its leg servos to step over obstacles while it is walking on rough ter- 
rain [Brooks 93]. 
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4. Video Camera 

Video input has been used as a robot sensor since the first robots were built in the 
1970’s [Meystel 91]. Video camera technology has the advantage of an extremely high 
sampling rate. A robot can detect and recognize objects at long range using video images. 
Modern video cameras now are available in extremely small packages with low power con- 
sumption. This supports small robots operating for extended periods in a harsh environ- 
ment. 

One of the primary disadvantages of video sensors is the computational expense 
required to process the video image. For this reason many robotic systems with video sen- 
sors process their video images on a separate ground computer. Video sensors are also sus- 
ceptible to variations in ambient light. This is a particularly difficult problem outdoors since 
scene lighting is highly variable. At night, most video systems are useless in the dark. Many 
modern systems currently use video sensors. The NASA/JPL Mars rover Rocky-V uses 
five video cameras for perception. The FINALE vision-guided mobile robot system con- 
trolled an indoor mobile robot at speeds of approximately 10 meters per minute in the pres- 
ence of obstacles [Kosaka 93]. This model-based system matched landmarks in the scene 
with features extracted from the images to perform self-localization. Odometry errors were 
corrected retroactively once the vision calculation was completed. This system was limited 
to stop and start motion since the robot had to be motionless to obtain an accurate video 
image. Additionally, the robot had to be provided with an accurate feature map of the world 
space. This system was computationally slow, since approximately 27 seconds were re- 
quired for one cycle of the localization process using a 16 million instructions per second 
(MIPS) computer [Kosaka 93]. 

From 1973 to 1981, Hans Moravec at the Stanford University Artificial Intelli- 
gence Laboratory developed a remote-controlled, TV-camera-equipped robot called Cart 
{Moravec 81]. The camera was remotely linked to a DEC KL-10 computer and the com- 
puter functioned as both the vehicle controller and as an image processor. The robot used 


stereo imaging to locate objects and to deduce its own motion. Distinctive features extract- 
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ed from the video images were used to perform a 3D analysis of the scene in front of the 


robot. The Cart robot represented obstacles with a map that was an ordered list of line seg- 
ments and empty areas. The empty areas were represented as convex polygon cells which 
included obstacle line segments. The system used this map to plan optimum paths that min- 
imized costs in terms of distance and energy requirements [Meystel 91]. 

The Cart robot was different from the Yamabico project in several important 
ways. The Cart’s principle sensor was a video camera, while Yamab.co sensor system uses 
12 sonars. Off-board computer processing was required for the Cart, whereas Yamabico is 
fully self-sufficient with regard to computing resources. Yamabico has the capability to 


recognize and utilize naturally occurring landmarks for odometry correction. 


5. Ultrasonic Range Finders 

Ultrasonic sensors have the advantage that they are low cost and readily available. 
Additionally, many examples exist in nature to demonstrate the effective use of high fre- 
quency sound waves for navigation. Bats, for instance, hunt their insect prey using ultra- 
sonics. Their sonar processing is far more sophisticated than any robot’s. Their main sonar 
energies used are in the 30 to 60 KHZ range. They can vary the time between sonar ranging 
pings to vary their range gate [Nachtigall 86}. Long ping intervals are used primarily for 
search and shorter intervals are utilitized for the terminal homing phase. Dolphins and other 
marine mammals use self-generated sound for echolocation of their prey underwater. There 
is evidence that the returned sounds are used for some kind of pattern recognition [Nacht- 
igall 86]. 

For mobile robot operating in air, ultrasonic range-finders do, however, have lim- 
itations. The speed of sound in air varies with ambient temperature, humidity, and baromet- 
ric pressure. Since ultrasonic range finders rely on time-of-flight, the variations in ambient 
conditions can affect range values. An ultrasonic pulse is transmitted by an acoustical trans- 


ducer and reflected back to the ultrasonic receiver by the nearest obstacle. Any factor that 
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affects the speed of sound in air, will affect the time of flight of the sound pulse. The speed 
of sound in fluids can be calculated by the Equation 2.1 [Kinsler 82], 


ada a 2.1 


where B is the bulk modulus of the fluid, c is the speed of sound in air, and p, is the fluid 
density. 

The ideal robot sensor is a pencil-thin, collimated beam that returns an accurate 
range to target regardless of the sensor beam’s angle of incidence. A collimated beam is a 
focused, parallel beam of transmitted energy. The wavelength of sound is long relative to 
light, consequently most target surfaces appear to be acoustic mirrors. Accordingly, surfac- 
es not nearly orthogonal to the direction of propagation reflect the signal energy away from 
the source and the surface is not detectable [Brown 85]. This is the biggest limitation of ul- 
trasonic sensors in mobile robotic. 

The piston source emitter commonly used for robotic range sensors tends to emit 
a wide sound beam. The sound pressure and sound intensity as a function of radial source 
distance R and angle @ with the acoustic axis can be calculated using the farfield expres- 
sions [Dario 86]. The directivity of the piston source may be expressed numerically by their 
3, 6 and 10 dB beam widths which are the angles 6 at which the intensity has dropped 3, 6, 
and 10 dB relative to the intensity on the acoustic axis. The sonar beam directivity is de- 
pendent upon the geometrical shape of the sound source and the frequency of the sound 
used. The range value returned by the sensor is the distance to the closest target anywhere 
within the emitter’s sonar beam that returns an echo with sufficient intensity to exceed the 
threshold of the receiver. A close, strong sound reflector may provide reasonable returns 10 
to 15 degrees off of the acoustic axis of the sonar beam. This results in poor overall direc- 
tionality since the sensor returns only a range value and no precise measure of the direction 


of the target is available. The position of the target reflector is calculated assuming that it 
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is centered on the acoustic axis. This results in position errors that, in some cases, exceed 
10 centimeters at a range of two meters (Leonard 92]. 

The target’s ability to reflect the incident sound wave is of crucial importance. 
Soft, sound-absorbing surfaces are poor reflectors. The target’s ability to reflect incident 
sound energy per unit area is called the target strength. A good example of a low target 
strength object is drapery on a window. Conversely, cardboard boxes have a high target 
strength. The experimentally determined target strength of various materials is given in 
Chapter II. 

Due to the finite amount of time required to transmit the sonar pulse, ultrasonic 
range finders have a minimum detectable range. This is due to the fact that the receiver can- 
not make a range measurement while the transmitter is transmitting. Sonar pulses are typ- 
ically one millisecond, which causes the minimum sonar range to be greater than 10 centi- 
meters [Dario 86]. Minimum range performance and cross-talk problems can be improved 
considerably when a separate emitter and receiver are used. Polaroid range finders have a 
single element that is both the emitter and the receiver. They are the most commonly used 
sonar system in robotics. 

Difficulties during the manufacturing stage may result in a large variation in pulse 
echo response of two commercial sensors purchased at the same time and said to be nomi- 
nally identical [Dario 86]. Slight differences in the sound pulse amplitude, pulse length, and 
pulse shape all contribute to differences between individual emitters. Differences in elec- 
trical connections, housing and material defects can also cause detectable difference in re- 
ceiver performance. 

Ultrasonic range finders are large with respect to some of the smaller robots in ex- 


istence today. For example, the Khepera™ miniature mobile robot at the Laboratorie de 


Microinformatique in Switzerland is 6.0 cm in diameter and 3.0 cm high [Modada 93]. In 
contrast, the emitter and receiver package for an ultrasonic sensor typically occupy a vol- 


3 


ume of about 100 cm”. Size is not an issue on large military-style robots such as the ALV. 
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Many smaller research robots use infrared range-finders which are significantly smailer, 
but have a shorter range. The IS Robotics™ R2 {Brooks 93] robot has five small infrared 


proximity detectors instead of sonars. 

The University of Michigan robot Carmel placed first among ten contestants in 
the 1992 AAAI Robot Competition. Carmel was a Cybermotion KA2 mobile platform with 
a ring of 24 sonar sensors. Object detection was performed using a color camera. Carmel 
used an error-eliminating rapid ultrasonic firing to accomplish fast obstacle avoidance 
while it navigated [AAAI 92a][Borenstein 92]. For mapping during the contest, Carmel 
used a global Cartesian system that stored only the location of the poles and the current po- 
sition of the robot. 

While at Oxford University, Leonard performed extensive research on model- 
based localization and map building using only ultrasonic range finders [Leonard 91]. The 
research involved the development of a specular sonar model for a rotating sonar sensor. 
The model predicted that clusters of strong sonar returns formed regions of constant depth 
(RCD). A RCD was a connected set of sensor returns with range differences less than some 
predefined range difference threshold. The RCD data was gathered by performing station- 
ary 360° scans at fixed intervals as the robot moved about the world space. No robot explo- 
ration was involved since the vehicle moved in a preprogrammed “seed spreader” pattern 
to gather the sensor data. Leonard used the Robuter robot and the SKIDS robot for his ex- 
periments. Ultrasonic range data was then processed off-line. In Leonard’s experiments, 
the aim was for a robot to maintain continuous map contact, “grabbing hold” of corners, 
planes and cylinders in the environment and then use them as handrails to guide the navi- 
gation process [Leonard 91]. Robot motion planning was accomplished off-line using a 
Voronoi diagram trajectory planner. 

Leonard’s robots used Polaroid sensors that had a significantly different beam 
pattern than Yamabico’s sensors. Yamabico uses collimated ultrasonic sonar detected with 
a separate emitter and receiver. Yamabico’s sonars tend to have a far narrower beam width 


and are therefore less prone to directionality problems. Also, Yamabico’s sensors have a 
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collimated receiver that tends to pick up less secondary specular reflections than the Po- 
laroid sensors. Leonard’s robot used RCD data to build its map while Yamabico analyzes 
line segment data derived from sensor data. This results in a lower computational complex- 
ity for Yamabico’s cartography algorithms. Typically, Leonard’s algorithm analyzed over 
1000 individual sonar retums [Leonard 91] to build a small map, whereas Yamabico pro- 
cesses about 15 extracted line segments (automatically derived from 1000 individual sonar 
returns) to perform the equivalent task. 


E. ROBOT LOCALIZATION AND NAVIGATION METHODS 

For a mobile robot moving in an unstructured environment, maintaining exact position 
informatic poses a major problem. Over long distances, dead reckoning estimates are not 
sufficiently reliable, consequently, motion solving methods that use landmark tracking or 
map matching are usually applied to reduce registration imprecision due to motion [Elfes 
87]. There are three basic types of localization dependent upon the map representation: 
model-based, grid-based, and local composite model [Leonard 91]. In the model-based lo- 
calization, correspondence is achieved directly between individual observations and the 
geometric model. For grid-based localization, an intermediate representation is built up 
from sensor range input and then correlated with the global grid-based map. The local com- 
pcsite model is similar since an intermediate representation is also built from sensor data 
and matched to the geometric world model. 

Elfes performed robot localization using an Approximate Transformation (AT) frame- 
work for robot localization with sonar data [Elfes 87][Elfes 90]. A robot motion M is rep- 
resented as M = (M’, E), where M is the estimated (nominal) configuration and E is the 
associated covariance matrix that captures the position uncertainty. E is applied periodical- 
ly to correct odometry errors. 

The Stanford Research Institute’s (SRI) robot Flakey placed second among ten contes- 
tants in the 1992 AAAI Robot Competition. Flakey was a custom built, octagonal robot 


with a circular array of 12 Polaroid ultrasonic sensors, an infrared laser and a charged cou- 














pled device camera (CCD) [AAAI 92a]. Flakey used a tolerant global map that contained 
local Cartesian patches related by approximate metric information [Davis 93]. Each of 
these patches contained a landmark or feature the robot used for localization. During the 
contest, the walls of the arena were used as landmarks and the approximate length and rel- 
ative orientation of the walls were given to Flakey as prior knowledge [Davis 93]. 

Flakey’s system was different from the one used in Yamabico since Flakey used toler- 
ant maps. With Flakey, large dead reckoning errors accumulated over just four or five 
meters of motion, especially when turning was involved. Yamabico has more accurate dead 
reckoning so its mapping is less error prone. Flakey stored patches of detailed grid-based 
landmark information linked together by tolerant metric data. These stored patches could 
be called features. In a way, Flakey used a hybrid map representation method that was grid- 
based near landmarks and feature-based with respect to landmark locations in the world 
space. Yamabico maintains a precise global map of the entire known world. Flakey used 
Polaroid sonars, while Yamabico uses Nippon Ceramic sensors. Finally, Flakey required 
some knowledge of the world space landmarks, whereas Yamabico does not require a pri- 
ori landmark knowledge to navigate in an unknown environment. 

Crowley performed localization by extracting straight line segments from sonar data 
and matching them to previously stored global line segment data [Crowley 86]. Cox imple- 
mented a continuous localization system using the robot Blanche [Cox 91]. Blanche used 
was an optical range finder sensor. Odometry updates were provided at eight second inter- 
vals with this system. Hinkel also implemented a localization system using a laser range 
finder sensor [Hinkel 88]. Sugihara [Sugihara 87] and Krotkov [Krotkov 89] performed vi- 
sual position estimation using vertical line segments as features. Using sonar data and grid- 
based map representations, localization has been performed by matching local occupancy 
grids with a globally referenced occupancy grid [Elfes 87] [Moravec 87]. Everett used spe- 
cial side-scanning ultrasonic range finders to detect walls and other obstacles in the robot’s 
environment for localization [Everett 93]. Curran used sonar and infrared range finder data 


to match expected and actual range values for localization [Crowley 89]. 
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F. ROBOT EXPLORATION AND CARTOGRAPHY 

If the geometry of the environment is not fully known to the robot system, one must 
employ an “exploratory” approach in which plan generation is tightly updated to gather 
data on the environment and to dynamically update the world model [Schwartz 88]. Robot 
exploration is the process in which a robot incrementally acquires and stores knowledge 
about the world space through intelligent motion and sensor input. In this section it is nec- 
essary to distinguish between two families of exploration schemes: undirected and directed 
exploration. Undirected exploration techniques explore the environment through random- 
ness. Directed exploration differs from undirected exploration in that the former utilizes 
some exploration specific knowledge for guiding the exploration decisions [Thrun 92], and 


actions are chosen based upon the maximum expected knowledge gain. 


1. Undirected Exploration 

Undirected exploration is an uninformed, random exploration technique. When 
selecting the next exploration action, no attempt is made to pick the action with the best 
expected outcome. The cost of the search or the reward associated with finding new, unex- 
plored space is not considered. Actions are selected stochastically based upon a uniform 
probability distribution for pure undirected exploration resu!ting in enhanced probability 
distribution for action selection such that the better the action, the higher the probability. 
Actions are still selected randomly. Undirected exploration is usually inefficient and the an- 
ticipated exploration time normally scales exponentially with the size of the space to be ex- 
plored. However, undirected exploration has been demonstrated as effective for some ap- 
plications. The robot Scarecrow used a random walk exploration technique to place third 
in the AAAI robot contest in 1992 [Dean 93]. 


2. Directed Exploration 
Directed exploration involves robot guidance based upon some specific knowl- 
edge or rules for searching. An exploration rule is used to determine the next action that 


best explores the environment. Directed exploration rules are heuristics since the robot is 
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exploring an unknown environment. Directed exploration techniques are usually more ef- 
ficient than undirected techniques in terms of time and energy to explore a given space. 

There are three general types of directed exploration; counter-based exploration, 
recency-based exploration, and error-based exploration. In counter-based exploration, the 
robot is driven to explore the least visited neighboring state next. Recency-based explora- 
tion favors the state which occurred least recently. Error-based exploration schemes make 
the assumption that states or regions in the state space with large error are little explored 
and demand further explanation [Thrun 92]. 

While at CMU, Thrun developed a system to explore and model an office building 
environment efficiently [Thrun 93]. He used the robot Columbus, a modified Heathkit ro- 
bot. This was an autonomous, wheeled robot with bumper sensors, a rotating sonar sensor, 
and wheel encoders for odometry. The exploration system used an instance-based learning 
technique for developing the map. Two artificial neural networks were used to encode the 
characteristics of the robot’s sensors and the characteristics of a typical indoor environment 
[Thrun 93]. One neural network encoded sensor interpretation and the other encoded con- 
fidence assessment. Exploration was achieved by navigating the robot to regions with little 
sonar data [Thrun 92]. The world model was represented using a grid-based mapping tech- 


nique with four inch grid squares. 


G. SUMMARY 


This chapter has provided an overview of the major challenges and issues with regard 
to mobile robot cartography. Map representation methods are primarily methods for a robot 
to store the world model information gained through sensor perception or prior knowledge. 
The size of the world model is a major factor since robot memory is a limited resource and 
restricts the size or resolution of the world the robot can understand. Additional world mod- 
el considerations are complexity and resolution. Robot motion planning issues are ad- 
dressed since a robot must move intelligently about in its environment to acquire sensor 


data to build a map. Motion planning is essentially a process that takes the current world 
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model as an input and gives a robot motion plan as an output. Software architectures for 


robotic vehicle control are tied to the robot’s morphology, mission, and map representation 
techniques. Hierarchical control gives a robot intelligence through centralized deliberation. 
These systems, however, are characterized by slow response to environment stimuli. Be- 
havior-based control gives a robot rapid, reflexive response to environmental stimuli, but 
limited intelligence. Hybrid control architectures represent an effort to combine the most 
desirable features of hierarchical and behavior-based control. 

The issues regarding robotic perception are reviewed by sensor type. Laser and light- 
type active emitter range finders have high data rate and give data relatively independent 
of ambient light levels. Contact sensing is limited by a robot’s ability to physically reach 
out and touch the environment. Video camera techniques have a high sampling rate, but are 
limited by the computational overhead required to process video images. Ultrasonic range 
finders are extremely popular with mobile robotic projects due to their low cost and avail- 
ability. The primary limitations are low sampling rate due to the speed of sound in air and 
the limited target incidence angle problem. 

Robot localization periodically corrects robot dead reckoning errors. The primary 
method of localization in mobile robotics is triangulation. Basically, sensor input is 
matched against some internal world model. The difference between expected sensor input 
and actual sensor input is used to derive the dead reckoning error. Robot exploration is nec- 
essary to transport the robot to all reachable portions of its world space for cartography. 
This is essentially a special purpose motion planning problem. The two basic methods are 
undirected exploration and directed exploration. This chapter reviewed the important issues 
and challenges of robot cartography to set the stage for the research that follows in the fol- 


lowing chapters. 
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Ill. THE YAMABICO-11 MOBILE ROBOT 


Yamabico-11 is an experimental, wheeled, autonomous mobile robot located at the 
Department of Computer Science, Naval Postgraduate School. Yamabico provides a test 
bed for robotic experiments and control theory development. This chapter describes the ro- 
bot hardware, and programming environment. A graphic-based mobile robot simulator de- 
veloped as a part of this research is also described. The simulator has been used extensively 
for prototyping new control algorithms and as a teaching tool for the Advanced Robotics 
course at the Naval Postgraduate School. 

Future robots are expected to possess advanced capabilities of sensing, planning, and 
control enabling them to gather knowledge about their environment. This knowledge will 
be stored as a model for planning and carrying out tasks sent to them in high level style by 
an applications programmer [Schwartz 88]. Yamabico represents this spirit of robotic sys- 


tem development. 
A. THE NAVAL POSTGRADUATE SCHOOL YAMABICO-11 


1. Hardware Description 

Yamabico-11 is an autonomous mobile robot powered by two 12-volt batteries 
and is driven on two main wheels by separate 35 watt DC motors. Yamabico is pictured in 
Figure 3.1. These motors drive and steer the main wheels while four shock absorbing caster 
wheels balance the robot. A VME card cage holds up to eight 6U-type Euroboard VME 
cards for on board computing hardware. The VME cage has a fan to dissipate heat from the 
computing boards. An Apple Macintosh Powerbook 145 notebook computer with an Artic- 
ulate Systems Voice Navigator voice interface is provided for user communications with 
the robot. 

An Ironics Sun3 single-board computer is the main processing unit. The master 


processor is an MC68020 32-bit microprocessor accompanied by an MC68881 floating 
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Figure 3.1 Yamabico 11 


point unit on a Mizar VME7120 board. This processor has one megabyte of main memory 
and runs with a clock speed of 16MHz. The processor has a bug monitor in ROM supplied 


by the manufacturer for basic programming and debugging. 


2. Sensor Characteristics 
The Yamabico sonar system consists of a sonar ranging board and a sonar array 
consisting of twelve Nippon Ceramic T40-16/R40-16 ultrasonic range finder emitter/re- 
ceiver pairs arranged around the robot’s perimeter. The ranging board is an Omnibyte 
OB68K VME I/O board that is controlled by an 8748 microcontroller. The sonar board is 
a separate input/output controller that makes the overall sensor process more efficient since 
the main central processing unit (CPU) does no sonar processing. This device has software 


programmable interrupts. This board takes the distance measurements from twelve sonar 
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Figure 3.2 Yamabico-11 Sonar Array 


sensors and presents the data to the CPU on the Yamabico robot. The sonar hardware de- 
sign gives a range gate of 409 centimeters and a range resolution of | millimeter. 

The Yamabico sonar array is illustrated by a top view of the sensor’s positions in 
Figure 3.2. The sonar sensors are arranged in three logical groups, with four sensors in each 
group. Group 0 consists of sonars 0, 2, 5, and 7; group 1 consists of sonars 1, 3, 4, and 6; 
group 2 consists of sonars 8, 9, 10, and 11; and group 3 is a virtual group which consists of 
four fixed test values [Sherfey 91}. Ranging is done on a group basis to prevent mutual in- 
terference. All four sensors in a given group range at the same time. Ranging takes place 
independent of the VME bus CPU. The sonar system completes its measurement of a given 
group then generates a VME bus interrupt. The VME bus CPU reads the data from the four 
sensors in the group from registers on the sonar board. After the CPU reads the sonar data, 
the sonar system begins ranging measurements on the next group. The VME CPU selects 
which sensor group is active by writing to a command register on the sonar board. The so- 
nar board individually controls the sonar ranging among the three sonar groups in the sonar 
array. 

The sonar transducers operate at a constant frequency of 40 kilohertz. Assuming 


that sound travels in air at 340 meters per second, the time for sound to travel one millimeter 
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is 1 millimeter /(340000 millimeter/second) = 2.94 microseconds. Since the time for round 
trip travel is measured, each millimeter of range gives a 5.88 microsecond delay. The 
counter used to generate a one millimeter count must have a frequency of 1/5.88 microsec- 
onds = 170 kHz. 

A sonar ping is a 40 kHz burst lasting 500 microseconds that is emitted by all four 
sonar emitters in an active group. Starting at the leading edge of the sonar ping, the ranging 
counter starts counting using a 166.7 kHz clock signal. Each clock cycle, the counter value 
is written to four 16 bit registers. Each register corresponds to one of the four sonars in the 
active group. After each counter is incremented, each sonar receiver is checked to see if a 
signal has returned. When a signal is detected, the memory address for that sonar is locked 
out to prevent any further writes to the counter. This effectively records the range in milli- 
meters to the first return exceeding the receiver threshold. After 4096 counts, the counter 
halts. Each memory location contains an integer representing the sonar return in millime- 
ters. If no return is detected, the most significant bit is set in the memory on the 4096 count 
signaling an overrange condition. The end of the counting generates an interrupt on the 
VME bus CPU. The VME bus CPU performs a serial I/O transfer by reading the four mem- 
ory addresses containing the four range values. After the last address is read, the sonar sys- 
tem begins ranging on the next selected group. Each reading cycle takes approximately 24 
milliseconds. 

Ultrasonic range-finders have limitations. Since ultrasonic range finders rely on 
sound time-of-flight, the variations in ambient conditions of air can affect the reliability of 
range values. The speed of sound in air varies with ambient temperature, humidity, and 
barometric pressure. Therefore, the range value returned by the range finder can vary con- 
siderably based upon ambient conditions. 

Acoustic sound waves are limited by the speed of sound in air. The speed of sound 
in air is only 343 meters per second at 20° C at sea level [Kinsler 82]. Air temperature, pres- 
sure, and humidity affect this value. The time required for sound to travel a round trip from 


the emitter to the target and back determines how often the sensor can obtain range data. 
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Since Yamabico’s programmed maximum range gate is 409 cm, the frequency of the emit- 
ter’s ping can be determined by using 8.18 meters as the maximum round trip distance and 
the speed of sound in air at 20 degrees C. The robot’s ping interval for a single sonar can 
be computed using this round trip distance as follows: 

8.18 meters * 343 meters/seconds = 0.0238 seconds = 24 milliseconds. 

Given this range gate, a single sensor can receive 1/0.0238 seconds = 41.67 range 
readings per second. Obviously, the choice of a shorter range gate allows for a higher data 
rate. Consequently, sound based sensors have a data rate limited by the speed of sound in 
air when a fixed sensor range is desired. 

The ideal robot sensor is a pencil-thin, collimated beam that returns an accurate 
range to any object it is pointed at regardless of the angle of incidence. A collimated beam 
is a focused, parallel beam of sound energy. The acoustic wavelength is long relative to 
light, consequently, most target surfaces act as acoustic mirrors. Since most surfaces act as 


acoustic mirrors, some of the incident sonar energy is reflected away from the sonar receiv- 
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Figure 3.3 Sonar Incidence Angle versus Range 
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Figure 3.4 Yamabico Sonar Beam Width 


er by the target. This effect become worse with increasing range and increasing angle from 
the normal to the target surface. In Figure 3.3 the sonar beam maximum incidence angle for 
a valid reflection versus target range is plotted. Sonar is a non-ideal sensor since the sonar’s 
beam must be nearly normal to the target’s surface in order to obtain a valid range return. 
Accordingly, surfaces not orthogonal to the direction of propagation reflect the signal en- 
ergy away from the source and the surface is not detectable [Brown 85]. This is the biggest 
limitation of ultrasonic sensors on mobile robots. On Yamabico-11, target surface must be 
within approximately 15 degrees of normal to the incident sonar beam in order for sonar to 
return a range to the target. 

Yamabico’s sonar transmitters have beam collimators to focus the sound beam. 
This results in good directionality but imposes limitations on specular returns. The Yamab- 


ico sonar beam is much narrower than the Polaroid sensors. Figure 3.4 shows experimen- 
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tally derived sonar beam width data from one of Yamabico’s sonar sensors. Notice that the 
sonar beam is widest between 100 and 250 centimeters due to spreading losses. 

The target’s ability to reflect the incident sound wave is of crucial importance. 
The target’s ability to reflect incident sound energy per unit area is called the target 
strength. Soft, sound-absorbing materials have poor reflectance. A good example of a low 
target strength target is drapery on a window. In contrast, cardboard boxes have a high tar- 
get strength. 


cardboard box chair rightside sonar — 
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Figure 3.5 Yamabico Sonar Scan Data 
Figure 3.5 illustrates Yamabico’s sensor data extracted from four meters of robot 
motion. The robot moved down the center of a hallway while scanning objects on both sides 


with its range finders. Objects found in a typical office environment were placed along the 


left hand wall. The targets included a cardboard box and a wooden chair. Notice that only 


43 





line segment data is extracted from the sonar scan. Yamabico does, however, have the abil- 


ity to return global position data from the individual sonar retums [Sherfey 91]. 
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Figure 3.6 - Relative Material Target Strength 


Figure 3.6 shows a bar chart of experimentally determined target strength data for 
steel, wood, concrete, cardboard, and other objects using Yamabico’s ultrasonic range find- 
ers. The amplitude of the return from each object was measured using an oscilloscope con- 
nected to one of Yamabico’s sonar receivers. The maximum return at the receiver was three 
volts. The target strength was expressed as a percent of the maximum return at the receiver. 

Polaroid sensors do not have separate emitter and receiver units. Yamabico-11 
uses a separate emitter and receiver. The pulse length is 500 milliseconds and the minimum 
range is 9.3 centimeters. This feature is important for precise navigation through cluttered 


indoor areas. 





B. YAMABICO-11 ROBOT SIMULATOR 


The purpose of the Yamabico simulator is to allow for robot software development and 
testing without programming the robot. At the AAAI 1992 Fall Symposium Series the 
consensus of the group focusing on AI and mobile robotics bears out the importance of this 


approach. 


For robot navigation in an office environment... simulation to perform a large number 
of experiments economically and physical robots to verify that the simulated results 
hold up in reality is the best approach [AAAI 92]. 


The simulator provides an X Windows graphics display to the computer screen to al- 
low the software developer to determine if robot motion is correct. The simulator is written 
in a portable language to facilitate transfer to other host computers. The system runs on a 
variety of computers with MIT X-Windows [Johnson 92] and a ‘C’ compiler. The time ex- 
pended in building this simulator was well compensated by the time saved in software de- 
velopment. The simulator runs in faster than real time. This allows for simulating long ro- 
bot experiments more quickly than the actual robot run time. Simulations run on an inde- 
pendent workstation, so the software developer is not subjected to a number of limitations 
including battery life (currently Yamabico-11 lasts about six hour on a battery charge), ma- 
neuvering space (lab space is tight), availability (Yamabico is a one-of-a-kind robot with a 
dozen software developers involved in programming), and convenience (the developer can 
test new algorithms anywhere an appropriately configured workstation is located). Exper- 
iments can run overnight or over a weekend. The results can be quickly evaluated without 
a human operator having to physically watch the robot. A 2D graphics display is sufficient 
to allow the software developer to evaluate the robot’s behavior. This simulator was devel- 
oped on a Sun workstation since these are less expensive and are generally more available 
than special purpose graphic workstations. 

The simulator has also served as a teaching tool in the Advanced Robotics course of- 
fered at the Naval Postgraduate School. Students learn more efficiently when they can first 
practice their programs on the workstation before trying them on the actual robot. The de- 
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mand for experimental time on the robot is greatly reduced since most application devel- 
opment time is spent testing robot code on the simulator. 


1. Design Goals of the Yamabico Simulator System 

The Yamabico simulator’s primary goal is to faithfully execute all commands that 
the robot executes. This is to include all sonar returns that would be received by the real 
robot and specifically it should accurately model the ultrasonic sensors. Also the robot’s 
multi-tasking system must be faithfully modeled in simulation; this alone is a challenging 
goal in a hard real-time system. 

The simulator uses the same “user.c” file that could be compiled and run on the 
robot hardware with no modifications. Additionally, the same code should be used for the 
robot and the simulator where possible. Compiler flags are used to switch between simula- 
tor and robot code when necessary. 

Robot motion should be shown on the workstation screen frequently during the 
test run so the developer can see the path the robot is taking. This allows developers to 
quickly test new software and watch a five sided robot symbol move about in the simulated 
world space. The above goals were adhered to as closely as possible during the simulator’s 


development. 


2. Simulator Top Level 

The top level of the Yamabico simulator is the main menu display that is shown 
in Figure 3.7. It is a graphic screen device that allows the simulator user to select the next 
simulator function. This graphic main menu device was developed using NASA’s graphical 
user interface toolkit TAE 5.1 [TAE 90]. The CMPL button is the compile button. When 
any portion of the software is changed, this button can be used to compile and link the soft- 
ware. This button invokes an UNIX makefile which recompiles all modified code files. The 
EDIT button invokes the “vi” editor in the current directory for the file user.c. This allows 


the user to edit the “user.c” command file and quickly recompile the code. 
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Figure 3.7 Yamabico Simulator Menu 


The RUN button is used to run the simulator. This starts the program which dis- 
plays a graphic of the robot’s world space. The robot’s configuration is plotted symbolical- 
ly five times per second. The real-time plot of robot motion is more fully described in the 
next section and an example appears in Figure 3.8. The elapsed time and the robot’s con- 
figuration are displayed numerically on the screen. 

The PLOT button allows the user to see a complete plot of the most recent robot 
mission. This button invokes the “gnuplot” program [Williams 92] which plots the robot's 
entire trajectory and any sonar data obtained during the robot’s last mission. 

The INFO button displays a help file that gives instructions for new users. The 
EXIT button allows the user to quit from the simulator. An additional screen display pro- 
vides the contents of the instruction buffer for the user. This is particularly useful for de- 


bugging Yamabico system code. 


3. Utility of a Robot Simulator 

The simulator motion .’ot is show in Figure 3.8. This is an X Windows applica- 
tion program design for rapid prototyping and analysis of robot control algorithms. This 
plot displays the robot’s trajectory as it executes the user’s command file. The robot's cur- 
rent configuration is displayed in the upper left hand corner of the display. An outline of 
the robot’s world space is provided to allow the user to determine the robot’s current loca- 
tion in the world space. The world space can be easily reconfigured by changing a world 
input specification file. The robot is plotted as a five sided icon every 20 vehicle control 


cycles. This is equivalent to five times per second. Depending on the speed of the host com- 
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Figure 3.8 - Simulator Motion Plot 
puter for the simulator, the robot executes the motion commands much faster than the ac- 
tual robot. Many MML programs have been developed for Yamabico using the simulator 
to test the robot trajectory first. 

The instruction stack for a typically “user.c” command file is shown in Figure 3.9. 
The robot’s commands appear in the first column. Then the following columns show the 
commanded geometry for the command. For path elements this geometry specifies the con- 
figuration of each path element. The path element tracking is more fully explained in Chap- 
ter IV and Appendix A. 

Practically all robotic projects have some kind of simulator to provide an environ- 
ment for software development, thus allowing software developers to develop and test new 
software modules without physically testing them on the robot. Students in the advanced 
robotics course have conducted simple simulation experiments in order to leam the MML 


system. The simulator also allowed researchers in the MML design group to develop ad- 
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vanced robot control systems in simulation. One example is the CLIPS Yamabico simulator 
designed by Fish for obstacle avoidance [Fish 93}. Another examples involves the simple 
automated cartography experiments used to build a symbolic map of the robot’s world 


space in simulation. 





Figure 3.9 Robot Instruction Stack From Instruction Buffer 


4. Simulator Sonar Model 
The simulator’s sonar model is shown in Figure 3.10. A simple ray tracing type 
algorithm that simulates the expected range finder returns from Yamabico’s ultrasonic so- 
nars is used. The sonar model consists of 12 virtual sonar beams represented as line seg- 
ments. Each beam starts at the global position of the corresponding sensor and ends 4.1 
meters from the sensor on the sonar beam’s main axis. This distance corresponds to the 


maximum range of the real sonar sensors. The robot’s world is modeled as a doubly linked 
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list of line segments that represent the boundaries of the robot’s world. This world is a sin- 
gle polygon in which the robot operates. No collision modeling is included since it is un- 
necessary to support automated cartography experiments. 





11 
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Figure 3.10 Yamabico Sonar Model 


The ultrasonic sonar beam is a simple line segment. The algorithm does a simple 
segment crossing test and returns the sonar range to the intersection of the sonar beam and 
world space surface. An incidence angle of 15° from a normal to reflecting surface is re- 
quired for a valid range return. As the simulator moves throughout the simulated world 
space, the sonar beam line segments from the enabled sonars are tested to check if they 
cross any portion of the world model. If a beam segment and a world segment cross, then 
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the distance from the robot’s sensor to the crossing point is determined. The sonar beam 


incidence angle is computed then a range value is returned only if the sonar beam’s inci- 
dence angle is between 75 and 105 degrees. Sonars ping in groups just as the real robot so- 
nar. When one or more sonars are enabled, the simulator pings a group of sonars every three 
vehicle control cycles. This correctly simulates the actual vehicle sonar cycle of 24 milli- 


seconds. 


5. Simulator Fidelity 

Vehicle motion is modeled accurately by the vehicle simulator. There is, howev- 
er, no vehicle dynamic model so a simple kinematic model is used. Ultrasonic sonar is mod- 
eled in a simple fashion and accurately produces the same messages to the laptop computer 
interface as the robot software. 

The multi-tasking system that is interrupt driven on the real robot is not faithfully 
modeled in the simulator. As a result, some temporal ordering is improper. Specular reflec- 
tions are also not modeled, and only the range to the primary reflection from the closest vir- 
tual surface is returned. The true shape of the sonar beam is approximated by a straight line 
ray. This is a relatively good approximation based upon Figure 3.4. Researchers have done 
extensive study on the physical nature of the ultrasonic sonar beam [Kuc 87] but no good 


specular model exists in the literature. 


C. SUMMARY 

The Naval Postgraduate School robot, Yamabico-11, is introduced in this chapter to 
provide a basis for comparison. The hardware characteristics are introduced to provide the 
reader with sufficient background for the next chapter. This robot is the test bed odometry 
correction theory in Chapter V and the automated cartography studies developed in Chap- 
ters VII and VIII. All experimental results are reported in Chapter IX. 

The Yamabico simulator is an important tool for robot software development in the 
MML project. A simple, but valid, sonar model is included to develop model-based mobile 
robot navigation algorithms and automated cartography. It has also be used as a teaching 
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tool in the Advanced Robotics course at the Naval Postgraduate School. This simulator was 
first used to refine the path tracking algorithms for Yamabico. The simulator allows the 
software developer to quickly test modifications to the MML language without operating 
the robot. A plot of the robot’s current location is displayed on the workstation screen at 
regular time intervals. A final full trajectory plot is displayed at the end of the simulation 


Tun. 
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IV. YAMABICO SOFTWARE ARCHITECTURE 


This chapter describes Yamabico’s software architecture that was developed in part to 
facilitate automated cartography. Specifically, this architecture provides facilities for task 
scheduling, resource allocation, spatial reasoning, vehicle motion control, sonar control 
and input/output functions. The geometric module provides spatial reasoning utility func- 
tions that support higher level robot behavior such as path tracking and dead reckoning er- 
ror correction. The intended path of Yamabico is specified by a series of path elements. The 
motion control subsystem provides a user interface for controlling vehicle locomotion by 
tracking geometric path elements. The sonar subsystem controls Yamabico’s sensor hard- 
ware through a library of ‘C’ functions. Sonar data collection and processing are accom- 
plished in real time using these functions. The input/output subsystem is crucial for mobile 
robot troubleshooting and analysis. This subsystem provides functions for robot two-way 


data transfer between Yamabico and either a host computer or an on board laptop computer. 


A. TASK SCHEDULING 

As Yamabico’s control system, MML is a multitasking operating system that provides 
robot motion and sensor functions, allocates processing resources, and performs odometry 
functions. The various required tasks are assigned an appropriate priority depending upon 
their relative importance. Higher priority tasks will interrupt one or more running lower pri- 
ority tasks when required. This system is an effective implementation of a “round-robin pri- 
ority queue.” An explanation of the operating system task scheduling is necessary. The Mo- 
torola 68020 CPU has eight interrupt levels [Motorola 85]. Some of these interrupts are 
used to run vehicle tasks at various priority levels in the single CPU, multi-tasking system. 
Table 4.1 illustrates these vehicle tasks. The higher the interrupt level, the higher the prior- 


ity of the associated task. At the highest level is Yamabico’s reset button. This tasks over- 
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rides all other tasks, stops the robot and resets the CPU. Interrupt levels five and six are cur- 


rently not used. 

Interrupt level four is the highest-priority task that runs during robot operations. This 
important task is responsible for steering the vehicle. Every 10 milliseconds, the locomo- 
tion task interrupts all other lower priority running tasks and runs for approximately 2500 
microseconds. This task first reads the shaft encoders and computes Yamabico’s odometry 
configuration estimate. This is a dead reckoning technique since only internal devices are 
read. All path tracking computations are performed at this level. Next, the most recent 
odometry configuration is used to calculate the proper curvature (Kk) and velocity (v) for the 
vehicle. These parameters are used to determine the desired vehicle rotational velocity (@). 


A kinematic function calculates the desired left (v,) and right (vp) wheel velocities. This 


information is used to determine the necessary pulse width modulation commands for con- 


trolling the left and right wheel drive motors. 


Table 4.1 MML SYSTEM TASK PRIORITY 
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The vehicle’s notebook computer interface input/output task runs at interrupt level 


three. This task is responsible for printing information to the vehicle’s on board monitor 
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and reading input from the user entered on the laptop computer’s keyboard. Also, file trans- 


fer from the robot back to the host computer is controlled by this task. 

The vehicle sensor functions run at interrupt level two. This interrupt is triggered by 
range information that is placed in the sonar board register. When one or more vehicle so- 
nars are enabled, this transfers data from the sonar board back to the main CPU at 24 mil- 
lisecond intervals. When none of the robot’s twelve sonars are enabled, this task is disabled. 
Interrupt level 1 provides a debugger task which may be enabled to print status information 
to the on board computer when a change occurs. 

Interrupt level 0 is the lowest priority task. All other tasks can interrupt this task. This 
task reads the user’s functions from an input file user.c and fills the command buffer based 
on the user’s sequentia) commands and modifies system parameters based upon immediate 
commands. These commands are explained in greater detail in Appendix A. The sonar sen- 
sors are enabled and disabled at this level. Additionally, all of Yamabico’s navigation func- 


tions run at this level. 


B. GEOMETRIC MODULE 

Yamabico’s geometric module provides mathematical support for many required spa- 
tial reasoning tasks. There are three important components in this subsystem; assignment 
functions for specifying geometric variables, math utility functions for manipulating the 
geometric variables and path tracking geometric support functions for reasoning about path 


elements. 


1. Definition Functions 
The definition functions are a collection of ‘C’ functions used to specify geomet- 
ric variables. These variables are essentially records containing several floating point pa- 
rameters. The definition functions specify vehicle configuration variables as well as path 
element variables. A configuration variable represents an object’s configuration in the glo- 


bal coordinate system using a four element record. Path elements are represented using ei- 
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ther a four parameter configuration variable or a five parameter parabola variable. Appen- 


dix A provices additional details and examples of each definition function. 


2. Functions 

The path utility functions provide a library of routines for the algebraic manipu- 
lation of geometric variables. For example, the composition function is used to perform 2D 
transformations and the inverse function determines the algebraic inverse of a given con- 
figuration. These functions support algebraic manipulations for automatic dead reckoning 
error correction as described in Chapter V. Also provided are an assortment of utility func- 
tions for spatial reasoning math on board Yamabico. Examples include three types of nor- 
malize functions and a ceiling function. All of these utilities support path tracking vehicle 
control. 

The path tracking geometric support functions serve to connect individual path el- 
ements for smooth vehicle motion. This subsystem is composed of two types of functions 
which are related. The intersection point functions determine the crossing point of two se- 
quential path elements. These functions have also been adapted to handle the transitions be- 
tween non-intersecting path elements. The leaving point function calculates a proper depar- 
ture point for Yamabico from one sequential path element to the next [Alexander 93]. 


These functions are explained in more detail in section C of this chapter and in Appendix A. 


C. MOTION CONTROL SUBSYSTEM 


Precise motion control using the path tracking method of vehicle guidance is essential 
for accomplishing automated robot cartography. Yamabico maintains a record of it current 
location using distance information provided by its optical wheel encoders. A current 
odometry configuration is crucial for path tracking and automated cartography. Precise ro- 
bot motion control is accomplished. The path tracking method of robot vehicle control is a 
part of the Model-Based Mobile Robot Language (MML) developed principally by 
Kanayama [Kanayama 91a]. Basically, this method allows Yamabico to move by tracking 


Straight lines, circular arcs, parabolas, and cubic spirals. This control method smoothly 
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guides Yamabico during real-time, dead reckoning error corrections. Corrections result in 
smoother motion when Yamabico tracks a reference path element instead of a reference 


configuration [Kanayama 93]. 


1. Odometry Capability 

Yamabico’s software system maintains an estimate of its current configuration in 
a configuration variable q, which is called its odometry estimate. The odometry estimate is 
updated each vehicle control cycle using information obtained from Yamabico’s optical 
wheel encoders. A small set of user functions are provided for three purposes; (1) set Yam- 
abico’s initial configuration at the start of a user.c program, (2) read the current value of 
Yamabico’s odometry estimate and, (3) update the current odometry estimate. These func- 
tions provide automatic vehicle odometry correction capability for Yamabico. The theoret- 
ical details appear in Chapter V. The functions that accomplish these tasks are more fully 


explained in Appendix A. 


2. Path Tracking 

Path tracking means that Yamabico’s intended path is specified by a series of geo- 
metric path elements. Yamabico software control system is extremely convenient for the 
user since MML automatically calculates the appropriate transitions between sequential 
path elements. This frees the robot programmer to focus on higher level robot tasks such as 
path planning and strategic motion control. To reduce the overall complexity of the system, 
only certain geometries and path sequences are allowed. 

Previously, MML used a reference configuration model to steer the vehicle. Early 
experimental work for this dissertation on robot odometry correction revealed problems 
with this control model. Odometry resets that resulted in large changes in the current con- 
figuration caused non-smooth, jerky motion. These corrections sometimes resulted in a 
temporary direction reversal by Yamabico. This problem was particularly severe when the 
new odometry position fell behind the robot. A dead reckoning reset to a position behind 


the vehicle caused the vehicle to back up to regain the correct configuration on the Carte- 
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sian plane (Kanayama 93]. Yamabico also was programmed to accelerate to a higher speed 
than the current operating speed in cases where the reset configuration was ahead of the 
current configuration. These types of corrections required Yamabico to “catch up” to the 
correction configuration. This acceleration together with poor control of Yamabico’s in- 
stantaneous path curvature caused unacceptable wheel slippage that resulted in increased 
odometry error. This non-smooth motion control was unacceptable for automated cartog- 
raphy. 

A better way to specify robot motion is through a series of planar path elements 
that serve to define Yamabico’s intended path. Automatic transitions between path ele- 
ments provides smooth vehicle motion along the intended path. The available path ele- 
ments include straight lines, arcs (constant curvature portions of a circle), cubic spirals, or 
parabolic line segments. One advantage of path tracking is the vehicle odometry reset are 
performed with respect to a reference path element instead of a reference configuration. 
This method smoothly guides Yamabico along the specified path when the odometry esti- 
mate is reset. No change in speed is required to catch up to a reference configuration. Yam- 
abico corrects its tracking with respect to a linear reference path. This allows it to maintain 
constant velocity as it follows the intended path. The overall wheel slippage is reduced 


since the vehicle can maintain a constant velocity after an odometry reset. 


’ : _ dk 
Smooth path tracking is accomplished using the steering function as which con- 


trols Yamabico’s instantaneous path curvature. Since Yamabico’s configuration is repre- 


sented in terms of x, y, 8, and x. The steering function is given by Equation 4.1. 
dk 
ds = f(x, y, 9, k) 4.1 


A signed distance value y* is used to represent the shortest distance between 
Yamabico’s current configuration and the reference path. The sign of y* depends on Yam- 
abico’s position relative to the reference path. When y* > 0, Yamabico is to the left of the 


reference path and y* < 0 means it is to the right. Yamabico’s configuration projected onto 
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the reference path is called the image. Yamabico’s steering function can now be represent- 


ed as 
dxf 
as = f(y ,9, x) 4.2 


The differences in the current curvature and orientation is given as 


AK = K do Kimage 43 
A6 = Oia > image 4.4 


where 6,4, is Yamabico’s current odometry orientation and Kg, is the current 


odometry instantaneous path curvature. The proposed steering function is 


= = -(aAx+bA0+cy*) 4.5 


where a, b and c are positive constants. Equation 4.5 is equivalent to 


e+ aAK+bA0+ cy" = 0 4.6 


In order to find the critical damping conditions required for non-oscillatory vehi- 
cle motion, a special path is considered. Assume that Pref is equivalent to the positively ori- 
ented x-axis of the global Cartesian coordinate system, i.e. y = 0. In this case, the image 


orientation and curvature are always equal to zero. Thus 
=6@ = 0 4.7 
and 


y‘ =y 48 
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Assume that -5 <0< > then Yamabico’s path can be represented as 


y = y(x) 


then solving for the steering function in terms of y. 


y? “4 
@ = atan(y’) = Yost ne. 


— y” (L+y2)? -3y’y (14 y2) 


then assume 
yr«l 
and 
yy" ay” 


Equation 4.12 becomes the ordinary differential equation 
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4.10 


4.11 


4.12 


4.13 


4.14 


yy” +ay”+by’+cy = 0 4.15 
which when put into differential form is 
(D3 +aD?+bD+c)y = 0 4.16 


Since Equation 4.15 is a third order linear differential equation with constant co- 
efficients, it must have at least one real root. A non-oscillatory decaying solution to Equa- 
tion 4.15 is desired, therefore there must be three negative roots of D. A critical damping 


solution of Equation 4.15 must have a triple root, call this root -k where k > 0. Thus 


(D?+aD*+bD+c)y = (D+k)? = D?+3kD?+3PD+K 4.17 
where 

a = 3k 4.18 

b = 3k? 4.19 

c=k 4.20 


Under these conditions, there is only one degree of freedom in choosing the coef- 


ficients a, b and c. Equation 4.15 becomes 
(D+k)3y = 0 4.21 
and its general solution is 
y= (Gr +Bx+C) e 4.22 


where A, B and C are integral constants. A size constant, 5, may be defined as 
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4.23 


then s, has the dimension of distance. The size constant determines the distance 
Yamabico moves along the reference path before it reaches the path. A smaller size con- 
stant makes the transition distance smaller, therefore s, controls the sharpness of Yamabi- 
co’s trajectory. 

Figure 4.1 illustrates the method of path tracking control. Each vehicle control cy- 
cle Yamabico reads its wheel encoders and calculates its current odometry configuration 
Qo. This configuration is geometrically projected onto a current reference path element. The 
configuration projected onto the reference path is called the image. The signed distance y* 


of g, from the path elements is also determined. Each vehicle control cycle the image and 


y* are used to determine Yamabico’s instantaneous path curvature kK. 





Figure 4.1 Yamabico Path Tracking Control 
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Figure 4.2 shows Yamabico following a path specified by a single straight line. 
The path element p; = (x, y;, 87, K,) defines the intended path. The path element p, rep- 
resents a directed half-line. The (x;, y;) components of p, define the origin of the line, 6, 
gives the orientation with respect to the x-axis and «, represents the path element’s curva- 
ture. Each vehicle control cycle, Yamabico’s control program performs the odometry func- 
tion by reading its optical wheel encoders. The vehicle’s odometry configuration is calcu- 
lated using the distance traveled by the left and right wheels. This configuration is projected 
onto p, to give the vehicle’s image. The tracking algorithm then determines the necessary 
path curvature and wheel velocity to move the vehicle onto the path element p;. The size 


constant Ss, determines how rapidly the vehicle converges onto the current path. 


Sq controls the curvature 


Py = (1, 1, 84, Ky) 





Figure 4.2 Yamabico Tracking a Straight Line 


Figure 4.3 shows Yamabico following a path specified by two straight line path 
elements. These path elements are specified by the configurations p, and p2. Yamabico 


tracks along initially using p, as the reference path. The geometric module computes the 


point of path intersection while Yamabico is in motion and tracking path p,. Next the leav- 
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vehicle 





Figure 4.3 Path Tracking Line to Line 


ing point on p; is determined based upon the intersection and the size constant s,. This s, 


parameter determines the maximum robot path curvature during the transition between p, 
and p2. Yamabico follows the line p, until the image reaches the leaving point. Then it 
switches to tracking path element p>. In this manner the vehicle automatically determines 
the optimum transition point between any two path elements. This technique results in 
smooth line-to-line path tracking with no overshoot. The motion control subsystem sup- 
ports all vehicle motion required to explore an orthogonal world space for cartography. Cu- 
bic spirals and parabolic path elements have been implemented but are not used for auto- 
mated cartography algorithm. Vehicle kinematic theory, all MML locomotion functions, 
and the rules for path element transitions appear in the Yamabico User’s Manual provided 
in Appendix A. Appendix B provides all of the motion control source code. 











D. SONAR SUBSYSTEM 

The sonar subsystem controls Yamabico’s sonar hardware, sonar data processing, and 
data storage. The sonar subsystem was developed principally by Sherfy and Kanayama 
({Sherfey 91]. Several improvements were made to support the research described in this 


dissertation. 


1. Hardware Control 

Yamabico’s sonar hardware is extremely efficient because three dedicated sonar 
boards control the sonar sensors [Sherfey 91]. Yamabico’s main central processing unit is 
interrupted only when data becomes available from the sonar array. The sonar system pro- 
vides user interface functions that control Yamabico’s array of sonar range finders. At any 
point within a user’s program, any of the 12 sonars may be enabled or disabled. This allows 
the user to operate a given sonar only when necessary for a particular application. The latest 
range value for a given sonar may also be provided by a range function. A user’s program 
can also be forced to “busy wait” until some sonar-based condition is satisfied. This feature 
is particularly valuable for obstacles avoidance. For example, a user’s program could be 


written to wait until the forward looking sonar’s range is less than distance d, then stop. 


2. Calculation of Global Sonar Return 

The global position of the sonar target providing a sonar return to a given sonar 
may be automatically calculated in real time. This calculation is necessary for linear fitting 
which provides the input for the edge extraction portion of automated cartography. This 
portion of the system uses Yamabico’s current odometry configuration, the range value re- 
turned from a given sonar and the configuration of the sonar sensor with respect to Yam- 
abico’s configuration to perform this calculation. A sonar target’s location (Xp, Yg) in the 
global reference frame is calculated by this portion of the module. This feature is illustrated 
in Figure 4.4 and described in greater detail by Sherfey [Sherfey 91]. Using Yamabico’s 
current odometry configuration estimate q,, the position of the sonar receiver (x,, y,) 


mounted on Yamabico is calculated by Equations 4.24 and 4.25. 
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X, = X,+ offset (sin (phi+0,)) 4.24 


Y, = Yq + offset (cos (phi+®,)) 4.25 


The sonar range value d is used to calculate the global location of the sonar target 
using Equations 4.26 and 4.27. 


X, =X,+ d (cos (axis + @,) ) 4.26 
Y, = ¥,+d (sin (axis + 8,)) 4.27 


3. Least Squares Linear Fitting 

Linear fitting of global sonar data for a given sonar is performed in order to extract 
line segments representing sonar reflecting surfaces in Yamabico’s world space. The linear 
fitting algorithm examines each individual global sonar return and determines if it can be 
fitted to the current line segment. When ten or more global returns fall onto a straight line 
(with a user’s selected tolerance), the linear fitting algorithm builds a line segment for a par- 
ticular sonar. Linear fitting continues as long as sonar returns fall onto the line segment un- 
der construction. Linear fitting is terminated when one global sonar return fails to fall onto 
the projected line segment being constructed. Line segment data can also be manipulated 
during line segment construction as well as after the segment has been completed. The line 
segment data may be manipulated using pointers to the individual line segment data struc- 
tures. This is an important feature for automated cartography because sonar line segment 
data must be efficiently manipulated in order to build a partial world from sonar data. 











sonat.({X, Ys) 


Yamabico 


(Xp. Yg) 





Figure 4.4 Global Sonar Return 


Suppose n consecutive valid data points have been collected in a local coordinate 
system, (p;,..., P,), where p; = (x;, y;) for i = J,..., n. The moments m, of the set of points 
using are obtained Equation 4.28. 

n 

m= > Xi (O<j,k $2, and j+k<2) 4.28 

f=1 

Notice that 799 = n. The centroid C is given by 


(10 Or = 
oad (tod BCE 4.29 
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The secondary moments around the centroid are given by 


Mp2} 2 mio 430 
0= 2 (x;-H,)" = My - (=) 
id m 
My, = DY - BD) O;-B) = (“ne 4.31 
i=l 00 
n m 2 
= egy 2 es pense [ee 


The parametric representation (r, &) of a line with constants r and a is adopted. 

If a point p = (x, y) satisfies Equation 4.33, 
= xcosa + ysina (-"/2<as1n/2) 4.33 
then the point p is on a line L whose normal has an orientation @ and whose distance from 
the origin is 7 as shown in Figure 4.5. This method has an advantage in expressing lines that 
are perpendicular to the x-axis. The point-slope method, where y = mx + b, is incapable of 


representing such a case (mm = ©, b is undefined). 


P=) yw 


ae residual 





Figure 4.5 Representation of a line L using r and « 


The residual of point p; = (x;, y,) and the line L = (r, @) is X;Cosa + y,sina — Tr, 


Therefore, the sum of the squares of all residuals is given by Equation 4.34. 
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n 
S= ¥ (r-x,cosa — y,sina)? 4.34 


i=l 
The line which best fits the set of points is supposed to minimize S. Thus the op- 


timum line (7, a) must satisfy 


dS_ dS 
7 ag 70 4.35 


Figure 4.6 provides an illustration of Yamabico performing least squares linear 
fitting of global sonar data. Individual global sonar returns are used to fit a line segment 
representing the surface providing the sonar returns. At least three global sonar returns are 
required to start linear fitting. The line segment under construction is geometrically project- 
ed forward. Global sonar returns must fall within a certain residual distance of this project- 


ed line. If any global return falls outside this zone, linear fitting for the current line segment 


Line Segment Individual global sonar returns 
Built by Linear Fitting 


Global sonar returns 
that fall in this zone 
are added to the line 
segment. 


E 
Ss 
2 
i 
5 
ie! 


Yamabico 





Figure 4.6 Linear Fitting 
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is terminated and all moments get reset. Figure 4.7 provides an illustration of actual global 


sonar data fitted to a line segment. 
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Figure 4.7 Linear Fitting Applied to Global Svar Data 
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4. Data Logging 


The sonar subsystem also provides facilities for sonar data storage and retrieval. 
Sonar data may be logged in three forms; raw range data, global target position data, and 
line segment data. An interval function controls how often sonar data is logged. Raw sonar 
range data consists of the individual range values for a particular sonar for each sonar ping. 
Range values are stored as the range from the sonar detector to the target in centimeters. 
Global sonar data consists of the Gp, Yg) position of every sonar return received while data 
logging is enabled. The position of each global return is stored as a pair of floating point 
numbers representing the x and y locations of the sonar target in Yamabico’s global refer- 
ence frame. The segment data from linear fitting may be logged as a series of line segments 
extracted by a given sonar. The endpoints, length, and orientation of each line segment are 


logged. This is a compact method of sonar data storage since an individual line segment can 
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represent hundreds or thousands of global sonar position values. Logged sonar line segment 
data is used as input information for some automated cartography functions. All of the ‘C’ 
code for the sonar functions is listed in Appendix C. The individual sonar functions for glo- 
bal data position determination are explained in the Yamabico User’s Manual in Appendix 
A. 


E. INPUT OUTPUT SUBSYSTEM 


Yamabico’s input/output subsystem provides three important functions; screen input/ 
output via the on board laptop computer, facilities for downloading executable programs to 
Yamabico’s main memory, and functions for retrieval of sonar data collected by Yamabico. 
Currently all data transfers are via two 9600 baud RS232 serial ports on Yamabico as de- 
scribed in Chapter II. 


1. On Board User Interface 

Yamabico is a self-contained autonomous mobile robot. A textual user’s interface 
is provided in the form of a Macintosh™ Powerbook notebook computer installed on board 
Yamabico. The screen input/output portion of this subsystem provides functions for read- 
3 . information from and writing information to Yamabico’s on board computer. Floating 
point numbers, text, characters, and integers can be written to the laptop’s screen to provide 
the user with current diagnostic information while Yamabico operating. This feature is an 
extremely valuable tool for troubleshooting bugs in the robot’s code. Similarly, functions 
are available that read user input from the laptop’s keyboard. A “user.c” program can be 
written that periodically requests keyboard input from a human user. A good example of 
this is an application that allows a user to choose among several programs loaded in Yam- 


abico’s memory. 


2. Facilities to Download Executable Programs 


Functions that download robot programs from the host computer are also included 


in this subsystem. Executable robot code on the host computer is downloaded via a 9600 











baud RS232 serial line. The entire robot kernel (MML system) or just an user’s application 
program (a compiled “user.c” file) may be transferred to Yamabico’s main memory using 


these functions. Two Unix file transfer programs provide the necessary data transfer proto- 


cols. Detailed operating procedures for downloading code onto Yamabico are provided in 


Appendix A. 


3. Retrieval of Data Collected by Yamabico 

Several functions provide the user with the ability to transfer data collected by 
Yamabico back to the host computer for analysis. These functions fall into three categories; 
location trace data, logged sonar data, and cartography maps. Yamabico’s odometry con- 
figuration and other guidance parameters may be stored in main memory during robot op- 
erations. The input/output subsystem provides a function for transferring this data back to 
the host computer. In the same fashion, sonar data and cartography data may also be trans- 
ferred back to the host via a serial link. The input/output subsystem provides the essential 
link between the mobile robot platform and the researcher. Post mission analysis of the data 


collected by Yamabico is important for analyzing the success of a mission. 


F. SUMMARY 


The task scheduling module provides multitasking scheduling for Yamabico software 
system. System resources are efficiently managed by this scheduler. The geometric module 
provides geometric spatial reasoning functions for path tracking and low level vehicle mo- 
tion planning functions. This module supports the motion control subsystem. 

Path tracking automatically computes the proper leaving point to facilitate a smooth 
transition between successive path elements. This frees the user from the tedious task of 
specifying Yamabico’s motion between paths. This also allows Yamabico’s software to 
calculate a path to the goal by a series of abstract path element segments which are easily 
turned into motion commands. Path tracking reduces odometry reset error due to wheel 


slippage. This is especially important at higher robot velocities. An odometry reset with re- 








spect to a path element is far smoother than an odometry reset with respect to a simple con- 


figuration. Yamabico’s odometry correction theory appears in Chapter V. 

The sonar subsystem provides a user interface for controlling of Yamabico’s sensor 
array. Functions are provided to process and store sonar data in real time. The input/output 
subsystem provides the essential interface between Yamabico and the user. Functions are 
provided to send data to and receive data from Yamabico’s main memory. This data may 


be transferred to a Unix host computer or Yamabico’s on board laptop computer. 
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V. THEORETICAL B.\SIS OF VEHICLE ODOMETRY CORRECTION 


The vehicle odometry estimate correction method used to support robot cartography 
is explained in this chapter. This discussion is preceded by an introduction to several related 
odometry correction methods and a discussion of fundamental concepts relevant to this 
work. This algebra provides a useful abstraction since the vehicle’s configuration, odome- 
try error, and vehicle landmar}s can now be manipulated by simple algebraic equations. 

The 3D homogeneous tra':sformation groups and qu.‘ ernions are widely used in anal- 
ysis and design of robot man/pulators [Paul 84] {[Lozano-Perez 83] [Fu 87] [Rolfe 86]. 
Likewise, a 2D transformation group to represent positioning of rigid body vehicles placed 
in a plane is needed. However, the formulation given here is not merely the 2D version of 
existing transformation groups. The expression of a robot’s orientation 6 is designed to ex- 
plicitly maintain complete vehicle orientation information oeyond the range of [-7,7). This 
allows the vehicle’s orientation to store the vehicle’s motion history. This formulation has 
the same advantage as a 3D Lomogeneous transformation, i.e. translation and rotation are 
described in a single mathem..tical structure, the configuration. This algebraic system is a 
variation of the 3D homoget.ous transformation group. However, the system does not 
have a point of singularity, which was one of the drawbacks of the homogeneous transfor- 
mation. 

This chapter introduces a configuration algebra based upon group theory. It is used to 
calculate Yamabico’s positio:. and motion in the 2D pine. This algebra provides the re- 
quired coordinate transformi.1.on calculations for dead reckoning error detection and cor- 
rection. This computationally efficient method allows odometry error determination and 
correction in real-time. 2D planar transformations are not a new concept, but this technique 
makes odometry corrections riore amenable to human understanding. Therefore, it is easier 
to write and debug computer ode to perform these transformations. Group theory provides 


a well known algebraic frame work for 2D transformation calculations [Bloch 87]. 
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Odometry correction is p*rformed using only three elementary components. 
(1) q! - configuration in\ 2rse (the mathematical inverse of a given configuration), 


(2) e = (0, 0, 0)! - the ide itity configuration, and 

(3) the composition functon (a function for combining two configurations). 

Group theory provides a simple abstract algebra that makes calculus related to vehicle 
motion design and control transparent and easy, including the analysis of dead reckoning 
errors. This algebraic system /s implemented in the high level mobile robot language, MML 
for the autonomous mobile robot Yamabico-11 [Kanayama 88] [Kanayama 91a]. All the 
definitions and the basic functions (composition, inverse, symmetric property, and so forth) 


provide a powerful and simpl: user interface. 


A. THE TRANSFORMA" ION GROUP 


This section introduces thy 2D coordinate transformation algebra. Let R denote the set 


of all real numbers. A transfor mation q is 


where x, y,8e KR. 5.1 


Rx) 
Ml 


The set of all transformations is denoted by T (= 3), For instance, g7 = (2, 1, n/6)e and 


q2 = (2,4, n/4 yr are examples of transformations (mt me ins the transposition of a matrix 


M). Obviously, a transformation q is interpreted as a 2D coordinate transformation from the 


global Cartesian coordinate s: stem F,, to another coordinate system F as shown in Figure 
Dike 
Let gy = (X,,y},8,)7? and gq, = (x, y>,9,)! . The composition 91°92 of 


these two configurations is defined in Equation 5.2. 


1D 





X41} |X xy +X, COs@, —y,sin@, ‘ 
= es ; 5. 
94°99 = |91|°|¥2| = y, +x281N@, + y, cose, 
81} |95 0,+8, 


The equation q, = 4p is true if and only if x7 = x2, y7= yz and there exists an integer m such 
that 6, = 65 + 2nn. The interpretation of g,°q, in the domain of 2D coordinate transfor- 
mation is the composition of the coordinate transforma.ons q, and q4. The following is 


one of immediate results from the definition above. 


composition of q; and q2 
91°92 


2= (Xz, Y2, ty) 
q2 configuration 


\—" t) 


q) configuration 


F,, global coordinate configuraiion 





Figure 5.1 Composition 
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Corollary 5.1 For any y = (x,y,0)! eT, 


x| 10 
ly = y ° 0 5.3 
t 0} |8 
Therefore, a transformation (x, y, et can always be decomposed into a translation (x, 


y, ot and a rotation (0, 0, 9)! Notice, however, that in general q # (0, 0, @)? (xy, 07, 
since the composition function is not commutative. The composition of the example trans- 


formations stated above is giver by Equation 5.4. 


B 


a1. 

1}o}4 - |j2+ 2,/3 5.4 
x . 50 

6 Ta 


In order to use the cumpose function for transformation algebra, it is necessary to char- 
acterize its properties in terms of familiar algebraic properties. In order to use the compose 
function in the transformatior space T, it is important to prove that the set of all transfor- 
mations form a group with re. pect to the compose function. The following lemmas are re- 


quired to prove this fact. 


LEMMA 5.1(Closure Property) For any 47,4) € T, 


5.5 
91°97 eT 


Proof: Each component of g,°q, in Equation 5.2 is a real number. a 


The closure property means that the composition of any two configurations gives a 


valid configuration in the trar sformation space T. 
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LEMMA 5.2(Associative Property) For any q7. 45, 93 € T, 


5.6 
(9°99) °43 = q\° (93°93) 
Proof: 
x, +x,cos@, —y,sin@, x3 7 
(93°45) °43 = Ma +x,Sin@, + y,cos6,|/y3 : 
) 
: 8, + 8, 2 
xy +X 0088, -¥p8in8, +X cos (9, +8,) - Yasin (9, +8.) 
=|), +Xqsin, +}'70088, +X300s (8 179) +Y3sin (8, +8.) 5.8 
i 6 1797+93 
x\+ (Xp +X30038,, ~y3sin®,) cos, - (9+! x3sin8, +y30088,) sin® | 
=|y)+ (Xz +X300:8, -y3sin8,) sin® + ee ae ee 5.9 
8, + (8, +93) 
1||*2 +X30088,, -¥3sin8, 5.10 
Xqsin8, + 8 ° (qn? 
, Yat i — @ = 4, (4 q,) 
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LEMMA 5.3(Identity) For all q € T, 


0 0 
g°10| = |0;°¢ = 9 aa 
0 0 
Proof. If q = (x, y, 9yF 
x10 x 0} |x 
5.12 
yl |G] = ly] = [Olly 
6} 0 i] 0||9 
Therefore, (0,0,0)? =e is the unique identity element in T. a) 


The following Lemma dernonstrates the presence of the left and right inverse transfor- 


mations for each q € T. 


LEMMA 5.4(Right and Left Inverse) Let q = (x, y, 8) T be given. The left and right in- 
verse of q are given by the following equations. 


(1) The solution to an .quation Viet = eis 


-xcos6 — ysin@ 
Vef: = | vSin® — ycos® 
—0 


5.13 


(2) The solution to an equation 9°q,;,,, = € is 
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—xcos6 — ysin6 5.14 
Gright = | x8in® — ycos® 
-0 


Proof. (1) Equation qj¢,°q = e becomes, 


Xtep, + XC08),,,— ysinG,,, =0 515 
Viepe + X8inO,, -, ~ YCOSO, 5, =0 5.16 
Bien t 8 =0 5.17 


By Equation 5.17, 6,,, = —0 . By substituting in 0, eft Equations 5.15 and 5.16, 


lejt 
Equation 5.13 is obtained. 


(2) Equation 9°q;jgp, = € becomes, 


X+XpightCOSO — Y,ign,Sin® = 0 5.18 
Y + Xp ign SinG — Y,j94,C0S8 =0 5.19 
0+ 6 in = 0 5.20 


Simultaneous solution of Equations 5.15 through 5.17 yield Xjey, = Xrighy Vieft = Y right and 


Oeft = Srigh- Since the left inverse q7 and right inverse q2 are equal, there exists a unique 


I 


inverse gq’ foreachge T. 0 


LEMMA 5.5(Existence of a Unique Inverse) For any transformation gq (x, y, 6) t. 
there exists a unique inverse 
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—xcos@ — ysine 
—] ' 5.21 
q =| xsin6O — ycosé 


—6 


Proof. The configurations g) and qz in Equations 5.13 and 5.14 each represent the 
inverse of g and they are equivalent. 0 


As an example, the inverse of a transformation (4, 2, n/6)! is 


| 4cos = —2sin~ 
4 cose —2sing| = j- 1-2/3] [4.464 
2) _| oa x}. | 2-8 0.268 5.22 
X 4sing 2COS — 7 t 
6 _t ie 6 


6 
Theorem 5.1 (Transformativn Group) - The set T of wansformation is a group with re- 
spect to the composition opcration (°), denoted by <T,°>. 

Proof. The algebraic structure < T,° > satisfies the closure property by LEMMA 5.1, 
the associative law by LEMMA 5.2, the existence of the identity by LEMMA 5.3, and the 
existence of an inverse by LEMMA 5.4. Therefore < T,° > is a group. Oo 

Group theory together wi'h its associated properties provides a well-defined algebraic 
structure for 2D coordinate transformation calculations. The closure property gives an as- 
surance that the results of the composition operation give: an answer that lies with the 2D 
transformation space. The inverse property provides the ability to undo any algebraic op- 


eration. The inverse property simplifies dead reckoning error analysis. 


B. FUNDAMENTAL CONCEPTS 
A vehicle is placed in a 2D plane KR? with a global Cariesian coordinate system F ov Lhe 


vehicle has a body-fixed Cartesian coordinate system F .. ‘The x-axis of F, points out of the 
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front of the vehicle and the y-axis of F points out of the Inft side of the vehicle. Since the 
vehicle moves, Fis a function of time. The vehicle’s position in this plane is described by 
a configuration q = (x, y, ®)’, where (x, y) is the position of the origin of F , and @ the ori- 
entation of F’ in the global coordinate system [Lozano-Perez 79]. A vehicle configuration 
(x, y, 6)" can be interpreted as a transformation (x, y, 8)’ which transforms F, into F,. In 


other words, the vehicle’s configuration is a transformation from the global coordinate sys- 


tem F,, to the vehicle’s local coordinate system F_. Under this interpretation, group theory 


can be used for the control ana analysis of vehicle motion. There is a method for describing 
a vehicle’s motion by a sequence of configurations [Kanayama 91a]. A set of configura- 
tions is selected so that any path segment which is obtained by the smooth path planner 
passes through the constraints specified [Kanayama 88]. For instance, a path shown in Fig- 
ure 5.2 is described by a sequence of eight configurations, which are also shown in the Fig- 


ure 5.2. 


0} 2) | : : 4} {1} {1 
(q+ 91> 922 93> V4 452 16°97) = Ilo} lo beet acl Vel (71 171 19 5.23 
O} [O} 15 5| 15 m| |r} {0 





These configurations am: constraints to the path and should be selected so that each 
path segment obtained by the smooth path planner will not have a conflict with the envi- 
ronment [Kanayama 88]. In oder to specify the next configuration relative to the current 
vehicle configuration, it may be easier calculate a relative configuration rather than a global 


configuration. For instance, when the current vehicle’s configuration is g,, a relative con- 
oO 


figuration ris given so that the next configuration q} is given by the composition operation: 


5.24 


97 = 4% 7 











Figure 5.2 Smooth Path Generated by Configurations 


amp:e, each (absolute) configura 


tion q; is calculated by Equation 


5.25, 


5.25 





using a sequence of reiative configurations: 


2 ; a {2 A 3\ [0 
(rp rp tp ato '? = 110 O| it Q} |-2 5.26 
5; 8 T 
0 5 0) | 0 5 O| | % 


These relative configu<aions are also shown in Figure Sz 
Cc. EVALUATION OF ROBOT OQDOMETRY ERROR 
One unavoidable problem in controlling autonomous mobile robots is the accumula- 
tion of dead reckoning errors Cver time. If dead reckoning error becomes excessive, the ve- 


hicle may become lost. The vehicle configuration 4, estimated by the on board odometry 


function is called the odometry config uration. Consider a situation in which the vehicle’s 


odometry configuration is dy and its actual configuration is qg- If there is no odometry er- 
101, 9o = Iq Otherwise, there is a difference between the vehicle’s dead reckoning estimate 
and its actual configuration as shown in Figure 5.3. An error configuration € is defined 


such that 


L°q, = Iq 5.27 


This relationship is i}lustrated in Figure 5.3. In this figure the error coordinate system 


is displaced from the global coordinate system by €. By the same token, the vehicle’s odom- 


etry configuration is displaced from its actual configuration by el. That is, this vehicle’s 


best estimate of its configuration 7, is correct only in the “erroneous frame” €. When there 
is no dead reckoning efror, the error configuration € is equal to the identity e. lf q, and 4, 


are known, the error configuration € can be calculated by Equation 5.28. 
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0,71 5.28 


| 


Pre 
ol] = (2.768, 1.134, 6) 5.29 
i.e., the vehicle’s odometry configuration q 9=(21,0)7 is ccrrect if it is interpreted as a local 


configuration in €. The compose operator is omitted from this point until the end of this 
chapter such that ¢7°q7 = q)@>. 
An analysis of an error that is caused by a sequence of vehicle motion follows: a ve- 


hicle has traveled through a series of two configurations q,, and g 02 in this order. These 
are estimated by odometry. Lei g 9 be their composition as given in Equation 5.30. Also, let 
the vehicle’s actual movemert as measured by an outside observer be 9q7 and qj, where 
q is their composition. | 


= 5.30 
Io = 191702 


= 5.31 
Gq — %91%q2 


Then, by substituting into Equations 5.27, the equations for the errors €, €,, and €2 are 


determined by the following equations. 


Far ~ ©7401 - 
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Figure 5.3 Odometry Error Anauysis 


= 5.33 
9a2 = ©2402 


= 5.34 
Iq = €4% 
Therefore, if each odomeiry motion estimate and error are known, the total error is 
5.35 


—1 pons 


f= 44 (94142) (951% 2) 


ae -1 -1 _ e.g7l 5.36 
Ed Ret gg to24l = = ©1%01*2401 
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The involvement of g, in this equation makes the error analysis complicated. Similar- 


ly, the total error equation for n consecutive motions is as follows: 


e=e € € Fiat Pla ee a 
ed ea 1% on ont on tonto, n- 1-401 


-1 ~1 5.38 


e= SL gi aad pd at wn-1°°°%01 


Assume a special case in which q, 7 =...=p p.1 = i.€., after each component motion 
9 
Joi» the vehicle is commandec. to come back to the initial configuration. In this case, the 


error configuration becomes simply the composition of a!] the individual errors as shown 


in Equation 5.39: 


5.39 


This feature is particularly useful for a robot application that requires repeated motion 
though the same configuration. This allows the robot’s dead reckoning error for one circuit 


of motion to be corrected when the robot returns tothe starting point. 


D. MODEL-SENSOR-BASED ERROR DETECTION 


An algebraic configuration is useful for describing tl: position of a vehicle. A config- 
uration is also useful to describe the position of any object in the environment. For instance, 


Object A in Figure 5.4 may be assigned a body-fixed, local coordinate system F’, and its 


position in this world is described using this local frame. Furthermore, consider a situation 


in which an ideal sensor mounted on the vehicle senses the configuration of an object in the 
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Figure 5.4 Object Configuration 


environment. That is, the vehicle is able to sense the relative configuration of an object with 


respect to its own odometry configuration q, with complete precision. Therefore, the vehi- 


cle’s odometry error is effectively super-imposed upon the sensed object configuration. 
A method for determining vehicle odometry error by using an external landmark as a 


point of reference is required for odometry correction. In Figure 5.5. q), is the vehicle’s ac- 
tual configuration, which is urknown, and p,, is the actual configuration of an object A in 


the environment, which is obt..ined from an environmente] model. The odometry configu- 


ration q ¢ iS known, but contains an error €. The configuration p,, is the observed configu- 


ration of the object A, and may have some error, becau:e this observation is made by the 
ideal sensor on board using the odometry configuration q, as a point of reference. As dis- 


cussed in Section B, a possible difference between p, and p,, is due to the error € in odom- 


etry. 
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Figure 5.5 Vehicle Odometry Error Detection 


The vehicle actually senses the object A at p/,, but since the odometry estimate is at q_, 
it believes A is at p,. The error configuration €, is introduced as the algebraic difference 


between both p, and p, and between q, and qq as shown in Equations 5.40 and 5.41. 


5.40 


€q, = 4% 


5.41 


€Po — Pa 


From Equation 5.41, tie error configuration is 
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e= PoPo 5.42 


Since both p, and p,, are known, € also becomes known. Therefore, by substituting the 


right hand side of Equation 5.42 for € in Equation 5.40, the vehicle’s actual configuration 


may be calculated algebraicallv from known values as shown in Equation 5.43. 


eat Sh ed 
Va = ©Ig = PagPo Io 5.43 


Since pz is obtained by th2 model and p, is obtained by the sensor, Equation 5.43 for- 


malizes the principle of the m.del/sensor based odometry error detection. This principle is 
applied in Chapters VIII for ‘ocalization during automated cartography. The results ob- 
tained from odometry estimate correction experiments appear in Chapter IX. 

The use of algebraic constructs for representation of a robot’s configuration, a sensed 
objects configuration, and a r-bot’s dead reckoning error significantly simplifies 2D mo- 
tion analysis. This algebra is important since three simple constructs provide all of the nec- 
essary tools for dead reckoning error detection from sensor data. The expected configura- 
tion of an object and the conf.guration of the object calculated from sensor input are used 
to allow the vehicle to rapidly calculate its dead reckoning :rror. This error is applied to the 


robot’s current dead reckonin;, configuration and the error is corrected. 


E. RELATIONSHIP TO OTHER TRANSFORMATION GROUPS 


The use of the three-dir:ensional homogeneous transformation group is common in 
the robotics field [Paul 84]. T'1e general form of a homogeneous transformation is shown 


in Equation 5.44. 
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Ry, Ry2 Ry3 


T= Roy Ro. Ro3 ¥ 5.44 
R31 R3y R33 Z 
0 0 041 


where the left-top 3 x 3 submatrix represents rotation and the right-top 3 x 1 matrix repre- 


sents a translation. Its two-dimensional version is 


Ry, Ry3 x cos@ —sin@ x 
Tos ae fee 5.45 
[0 01 0 0 1 


This transformation matrix T> may be used to represent vehicle’s configuration as de- 


scribed in Section C. In ordes to obtain @ itself from T, Equation 5.46 is used. 


@ = atan2(R,,, R11) 5.46 


where the range of the function atan2 is assumed [-7, 2]. However, this method has a draw- 
back. If the vehicle’s accumul ited rotation is beyond the range of [-7, 1], a part of the ve- 
hicle’s orientation information is lost. For instance, if the vehicle rotates 2x counterclock- 
wise, 8 becomes 0 instead of Z:t if Equation 5.46 is used. In order to avoid this information 


loss, an explicit 8 term should be added to T2 obtaining 7: as shown in Equation 5.47. 


| cos —sin@ x 0 


T. =|sin® cos@ yO 5.47 
3°} 0 0 10 
Lo oO 01 
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The set T’ of all 4 x 4 matrices of the form in Equation 5.47 under matrix multiplica- 
tion is a subgroup of the group of all invertible 4 x 4 matrices. The symbol (x) means matrix 
multiplication. Therefore < T’, X> is a group. 

Groups may be modified but not really but not really changed. When two groups look 
different but are essentially the same mathematically they are said to be isomorphic. The 
concept of isomorphism is defined as follows: 

Definition: Let <G, #> and <H. $> be groups. G is isomorphic to H provided that there 
exists a function @: G — H such that 

1. Ois 1-1. 

2. © is onto H. 

3. © preserves the operation; that is (a#b)O =(aO)$(bO ) forall a, be G. 


Proposition 5.1 < T, °> is isomorphic to < T’, X>, the subgroup of the multiplicative group 


1 0 0 O 
x cos@ —sin@ 0 
y sin@ cos@ 0} 
6 0 0 1 


of 4 x 4 invertible matrices consisting of all matrices of the form 


Proof: The transformation group < T, °> is isomorphic to a subset of the 4 x 4 invert- 


ible matrices under standard matrix multiplication. Let f(q) be a function that maps each 


q=(x, y, ®)' € <T, °> to the matrix in Equation 5.48. 


1 0 0 0 
x cas@ —sin€ 0 5.48 
y sin® cos@ 0 
6 0 0 1 


Clearly, f(q) is a 1-1 function that maps onto every 4 x 4 matrix of the form above 


where x, y, 9 € R. In addition, f( q; ° ¢2 ) =fK gq; ) Xf 42 ), where is x the usual matrix 
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| multiplication. Therefore f preserves the operation and is a homomorphism. Since fis 1-1 


and onto, < T, °> is isomorphic to the subset of 4 x 4 invertible matrices of the form 


1 O 0 0 


a cos —sin8 0 , Where a, b, 9 Ee R. 5.49 
b sin® cos@ 0 


6 0 0 1 


To show that f is a homomorphism, let q7 = (x), y;, 8 Ai and q2 = (x2, y2, @,)F be ele- 


ments of < T, °>. It follows that 


X)||X5 xy +x,C0S6, — y sind, 


: 5.5 
94°99 = v2 = y, +x,sin®, + y, cos, 0 
6,!|8 
\ 2 6, +98, 
Therefore 
1 0 0 0 


e ' x; +x,C0S0, — yz sine, cos (8, + 8.) —sin (@, +8.) 0] 5.51 
q °q = - < 
142 y, + x,8in8, + y,Ccosé, sin (8, + 8,) cos (6, + 8,) 0 

8, +6, 0 0 1 


It follows that 


1 0 0 oli o 0 0 
aT ee 1 cos6, —sine, 0 X» cosé,, —sin6, 0 : 5.52 
so | ames sin@, cos@, O||y, sin@, cose, 0} 


8, 9 0 1/6, 0 0 1 
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1 0 0 0 

xy +x,C0S6, — y,sine, cos (8, +8.) —sin (@, +6,) 0 5.53 
y, +x2Sin®, + y,cos®, sin (6, + 65) cos (0, +8.) 0 
6, +6, 0 0 1 


using the trigometric identities for the sum of two angles. QO 

Although the groups < T, °> and < T’, X> are.isomorphic, operations in the transfor- 
mation group < T, °> are simpier to represent and are computationally more efficient than 
the matrix multiplication in < I’, X>. This computational efficiency is essential for trans- 
formation calculations performed by an autonomous robotic platform. Obviously, this 


transformation system does not have any singularity. 


F. SUMMARY 

Dead reckoning error correction is essential to automaced cartography since significant 
robot motion is required to map a world space. All accumulated vehicle error must be rec- 
onciled during automated cartography motion or the resulting map will be useless. A 2D 
configurational algebra based upon group theory provides a solution to the accumulated ve- 
hicle error problem. The group theory in this chapter provides an elegant algebraic structure 
that makes robot motion calculations more transparent. Motion analysis and robot dead 
reckoning error determination are made possible using this algebra. This system is com- 
plete in the sense that for every possible situation, the robot’s configuration and its dead 
reckoning error can be represented. These properties arise. 1rom group theory which implies 
all configurations have a unique inverse and there is no singularity on any configuration. 

One of the open problems related to this theory is whether there exists a similar theory 


in three dimensional transformations. i.e., how can the “composition” and “inverse” for 


transformations q = (x, y, Z, 9, 9, yw! be defined. Here, 6, 8 and w are Euler angles [Paul 


84]. The periodic detection ard reduction of odometry errors allow an autonomous mobile 
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robot to work for a sustained period with great precision. This theory has been experimen- 
tally validated using Yamabico-11 and the results are described in Chapter IX. 
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VI. REPRESENTATION OF THE WORLD 


The automated cartography algorithm employs a polygonal map representation. This 
has also been called abstract mapping since the computational complexity is related to the 
number of features in the robot’s world space instead of the number of grid squares. Le- 
onard called this type of map a feature-based map [Leonard 91]. The relatively low com- 
putational complexity required to build an abstract map enables a robot to perform the au- 
tomated cartography in real time using on board computing resources. Cartography using 
only on board computers is more efficient since there is no communication delay associated 
with communications to another computer and there are no inherent range restrictions. In 


this chapter, the abstract data structure for the world representation is defined. 
A. REPRESENTATION OF A POLYGON 


1. Example Polygons 
Polygons are used as the means of representing free space for this dissertation. 
Polygons are the basic building blocks for the world representation. A polygon is a collec- 
tion of vertices connected by edges that divide the Cartesian plane into two regions. In Fig- 
ure 6.1, several example polygons are shown. The simplest polygon is a triangle, since a 
polygon must have three or more edges. All polygons in this dissertation are simple, to dis- 
tinguish them from polygons that cross themselves. A polygon with 7 vertices is called an 


n-gon. 


2. Definitions 
A vertex is defined as a point in the Cartesian plane such that v = (x, y). An edge 
is a directed line segment designated by e = {v, v’, type) which represents an ordered pair 


of vertices (v, v’) and a type (“real” or “inferred”). An example of an edge is 
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Figure 6.1 - Example Polygons 


€2={V2,V3, “real” } in Figure 6.1 (c). The edge e2 = {v2, v3, “real” } consists of a vertex pair 
(v2, v3) and a type = “real”. 

A polygon P = {E, e, next} is a tuple containing a set of at least three edges E = 
(€,, €, ..., @,) , a first edge e, and a next function. The edges and the next function are se- 


lected such that no pair of nonconsecutive edges share a point [O’Rourke 87]. The next 




















function defines two types of polygons based upon the order of the edges in the polygon; 
hole-type and boundary-type. For hole polygons the edges are directed in counterclockwise 
order and for boundary polygons the edges are directed in clockwise order. For example, 
in Figure 6.1 (b), polygon P,, = {{e), €2, €3, €4}, €), next} is a hole type polygon with four 
edges that are directed counterclockwise. In Figure 6.1 (c), polygon P, = {{@7, €2, €3, eg}, 
e}, next} is a boundary type polygon with four edges directed clockwise. The domain of the 
next function is closed with respect to the polygon’s edge list. The vertices of any sequential 
edges are coincident such that if next(v;, v2) = (v,’, v2’) then vz = v,’. The next function is 
an one-to-one function such that every edge has a predecessor. A polygon with n edges is 
a closed, directed cycle such that (Ve) next"(e) =e. 

The orientation of an edge is defined as the angle a directed edge makes with the 
x-axis as illustrated in Figure 6.2. The orientation of a given edge e = {v7, v2, type} is given 


by the function (e). The external angle Y of a given edge is the angle formed by the ex- 


WV}, V2, type) 





Figure 6.2 - The Orientation of an Edge 


tension of a given edge and the next edge of a polygon. Expressed mathematically, the ex- 


ternal angle is; Y; = W(next(e;)) - W(e;). The external angle concept is illustrated in Figure 


6.3. Kanayama proved that the sum of the external angles of all the edges in a polygon is 











Figure 6.3 - The External Angle 
equal to +27 in accordance with the equation Ly, = +27 [Kanayama 91). For hole-type 
polygons LY; = +27 and for boundary-type polygons Ly; = —21. 

The collection of edges and vertices is referred to as the boundary of P, denoted 
by OP. The inside of a polygon is defined as the infinite set of points such that a non-oscu- 
lating, directed half line drawn from any point intersects the boundary of the polygon an 
odd number of times [Kanayama 91]. This concept is illustrated in Figure 6.4. The point p; 
is inside the polygon P since the directed half-line L; intersects the polygon P three times. 
Another directed half-line (L2), also drawn from point p;, intersects the polygon P once. 
The point p3 is not inside (it is outside) of P since its directed half-line (3) intersects the 
boundary of P an even number of times. 

The free area of a given polygon is the planar region defined by the boundary of 
that polygon. For a hole-type polygon the free area is the planar region to the right of any 
given edge and therefore outside of the region enclosed by the polygon. The region inside 
any hole polygon is defined as filled and the region outside is the free area. For example in 
Figure 6.1 (b), the filled side of the hole polygon P,, is the shaded region inside of P,, and 
the free area is the white region outside of P,. For a boundary-type polygon the free area is 


also the planar region to the right of any given edge and inside the planar region enclosed 
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Figure 6.4 - Region Inside a Polygon 


by the polygon. The free area of a boundary polygon P is the region inside OP and the filled 
area is the region outside OP. This concept is illustrated in Figure 6.1 (a). The free area of 
boundary polygon P, is the white region inside OP, and the filled area is the shaded region 
outside OP,. The function free(P) gives the free region for any given polygon P and the 
function filled(P) gives the filled region of any polygon P. 

An orthogonal polygon P is defined as a polygon whose edges are all aligned with 
a pair of orthogonal coordinate axes, which without loss of generality are taken to be hori- 
zontal and vertical [O’Rourke 87]. The polygons Pq and Py in Figure 6.1 are examples of 
orthogonal polygons since their edges are all orthogonal to a pair of orthogonal coordinate 
axes. In Figure 6.1 (d) polygon Pg is an orthogonal hole polygon and Py is an orthogonal 
boundary polygon. 

The edge type designation is used for recording robot sensor input. “Real” edges 


are derived when the sensor observes some visible edge of its world space W by direct sen- 
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sor scan. “Inferred” edges are constructed edges that serve to bound unexplored or occluded 


portions of the world space. 


B. REPRESENTATION OF A WORLD 

Chapter VII describes the development of a series of three idealized robot cartography 
algorithms. These algorithm provide a firm theoretical basis for the real automated robot 
cartography algorithm that follows in Chapter VIII. Polygonal regions are used to represent 
the free space mapped by the automated cartography algorithm. This section gives some 
examples of world representations and defines the basic terms used in the algorithms. 


Worlds are compound polygons with the hole polygons used to represent obstacles in the 


world and with a single boundary polygon used to represent the exterior boundary of the 


known world space. 


1. Example Worlds 


In Figure 6.5, three example polygonal worlds are shown. The shaded area repre- 
sents the free space bounded by each world. W, is a world consisting of a boundary polygon 


with six edges and no holes. W, is a world with one hole H;. W, is a world with two inferred 
edges and no holes. Notice in W, that the edges of the boundary polygon P,, are directed in 
clockwise order. Further notice that W,’s hole polygon H, has edges directed in counter- 


clockwise order. 


2. Definitions 
A world W is defined by the pair W = {P,, H) such that P, is an exterior boundary 
polygon with a sequence of zero or more simple polygonal holes H = {H), ..., H,}. The 
free a.za of the world free(W) is a multiply connected free space with h holes: it is the re- 
gion of the plane inside of P,, but outside of H;, ..., Hy. The polygon P, is a boundary- 
type polygon with edges directed clockwise. The hole polygons H), ..., H), edges are or- 


dered such that the edge number increases clockwise around each polygon as shown in Fig- 

















Figure 6.5 - Example Worlds 








ure 6.5 (c). In this manner, the edges are directed line segments such that the left side of any 
given edge is the filled area of W and the right side of any edge is the free area of W as 
shown by the directed edges in Figure 6.5. 

Let the area of the Cartesian plane bounded by a polygon P be represented by | P| 
and let |P,| \|P.2| mean the intersecting planar area of polygons P; and P2. Let free(W) 
represent the intersection of the free areas of all component polygons in the world W. The 
area bounded by a world W is the intersection of the area bounded by the world’s boundary 
polygon P, and all A of the world’s hole polygons H), H2,...H), as given by Equation 6.1. 


free(W) = free(P,) Ofree(H,) 0... V free (H,) 6.1 


Equations 6.2 and 6.3 define the required conditions for a world. Equation 6.2 
means that no hole polygon may intersect the boundary polygon. Equation 6.3 means that 


no two hole polygons may intersect. 


(Vi) (filled(P,) A free (H;) = @) 6.2 


Vi, i{G #j) = (filled (H,) Ofilled(H))) = gj 6.3 


An orthogonal world W = {P,, H} is defined as a world with all edges aligned to 
the same pair of coordinate axes. In Figure 6.5, W, = {P,, H} is an o. thogonal world, where 
H = {H;} is the polygon hole list containing one polygon H; and P,, is the boundary poly- 
gon. 

A partial world PW(W) of the world W is a polygon with holes such that PW(W) 
represents some region inside of W such that free(PW) & free(W). For the purpose of these 


definitions, inside is defined as any point in the planar region within the free space of the 
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boundary polygon and not within a hole polygon as given in Equation 6.1. The edges of 
PW(W) may be either “real” or “inferred”. The “real” edges are the edges shared by W and 
PW(W). The “inferred” edges are edges whose vertices both lie on edges of W and separate 
a region enclosed by both W and PW(W) from a region enclosed exclusively by W. Thus, 
the boundary of the partial world 0PW(W) must lie entirely within W. The null partial map 
PW(W) = @ of the world W is a partial world such that it has no holes (h=0) and the number 
of vertices in P, is zero. A completed partial world is a partial world such that PW(W) = W. 


PW is said to be a correct partial world of W if and only if free (PW) cfree(W). 


C. SUMMARY 

This chapter develops a method for world representation suitable for automated robot 
cartography. A polygon is the basic structure for representing a free space. A world is a 
complex polygon with one exterior boundary polygon and zero or more non-intersecting 
hole polygons all inside of the boundary polygon. The polygonal method of world repre- 


sentation is used for storing the current world model during cartography. 
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VIL THEORY OF AUTOMATED CARTOGRAPHY 


Mobile robots that function autonomously in the real world have been a major objec- 
tive within the Artificial Intelligence community since the first days of work on intelligent 
systems. The ultimate goal has been a robot that would navigate through an environment, 
managing to both learn the layout and to perform assigned tasks [Moravec 81]. To develop 
a firm basis for robot automated cartography this chapter examines robot cartography using 
a point robot with an idealized sensor. This theoretical examination is initially unencum- 
bered by robot motion error or the sensor limitations described in Chapter II. This problem 
abstraction allows a theoretical examination of the proper representation for the robot’s par- 
tial map and the proper robot motion planning to support automated cartography. 

For automated cartography, the issue of how to sense the geometrical relations of the 
world is the central theme. In this chapter, three progressively less idealized sensors 5), S2, 
and S; are introduced and a cartography algorithm for the idealized robot using each ideal- 
ized sensor is presented. In each algorithm, one additional characteristic of the real ultra- 
sonic range finder’s sensor is added. S, is an idealized, infinite range sensor that returns in- 
formation regardless of target incidence angle. The S> sensor has infinite range capability 
but edge detection is limited by sensor beam incident angle. Finally, the S; sensor has both 
finite range and limited sensor beam incident angle capability. Each case study provides an 
algorithm and a proof of correctness. The progressively less idealized sensor algorithms 
serve to clarify the logical structure of the complex algorithm for the real robot. The ideal- 
ized robot R used for this theory has perfect motion control precision so that the algorithms 


stated above give a map with a precision determined by the range sensors. 
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A. ALGORITHM FOR IDEALIZED SENSOR S, 


The idealized robot R is initially placed at an arbitrary location inside of W. R may 
move freely to any point inside of W with no dead reckoning error. This implies that R al- 
ways has perfect knowledge of its position in the global coordinate system regardless of the 
distance it has traveled since its initial placement in the world. A robot R is defined as a 


point robot. The only constraint on R is that it must remain within free(W). 


1. Characteristics of the S; Sensor 
Idealized Sensor S, - The sensor S; moves about in free(W) attached to R. This 
perfect sensor S; is a ray-tracing range finder [Leonard 91] with infinite range. S; returns 


the edges of any portion of W’s edges regardless of the sensor beam incidence angle as long 


as the edge is visible from R. At any point x in W, S; is swept through an arc and extracts a 


list of edges Q from W. 
Rhas a single idealized sensor S, that is, fundamentally, an edge detector. All edg- 


es both visible from R and detectable by S, are said to be illuminated or detected by S,. 5, 
operates either by sweeping a circular arc from R’s current position or by scanning left and 
right perpendicular to R’s path while R is in motion. R may remain stationary or move while 
S, is operating. S, has an edge-extracting capability such that any edge or portion of an edge 
of W that is illuminated by S, will be returned to R as a “real” edge. 

In Figure 7.1, R sweeps the S, sensor about a 360° arc at point C). All edges vis- 


ible from R are extracted. A boundary polygon for the first partial world PW is formed from 


the edges extracted from the C; sweep. Notice that edges e, and e, are not visible from 
point C;. An “inferred” edge e3 is constructed to connect the discontinuity between e2 and 
eq. R moves to point Cz and performs a second sweep which reveals edges e, and e,. The 
“inferred” edge e3; may be removed since the region beyond it has now been scanned. 


In the special case, an “inferred” edge may turn out to represent a real boundary 
surface as shown in Figure 7.2. In this figure, edge e; happens to be one of W’s edges. In 
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Figure 7.1 Idealized Robot Cartography Sweep 





Figure 7.2 Idealized Robot Cartography Sweep Special Case 


Figure 7.2, R obtains a partial world PW, at position C;. The partial world boundary poly- 
gon PW;°P, kas five “real” edges e7, €2, €4, €5, €g and one “inferred” edge e3 since 5, de- 
tects a discontinuity between edges e2 and eg. R moves to Cz and performs a second sensor 
sweep. This sweep does not reveal any additional free region beyond edge e3. The sweep 


does, however, reveal that edge e; is in fact a “real” edge and therefore part of W. This fact 


107 











was impossible to verify from the position C; since C; happened to lie on a line extended 
from e; to the left. Therefore, as long as “inferred” edges remain in R’s partial world, R 
needs to move to obtain additional sensor data from another viewpoint. The necessary mo- 
tion to optimize R’s view of an occluded region is a fundamental component of intelligent 
robot exploration. 


The algorithms for the idealized sensors are now described. The S; sensor is a per- 
fect robot sensor with infinite range for target detection. S, also extracts edges regardless 
of sensor beam incidence angle. The S2 sensor also has infinite range capability, but ex- 
tracts edges within a limited sensor beam incidence angle. The S3 sensor is further limited 


by both finite range and limited sensor beam incidence angle. Further abstract sensors are 


unnecessary since the important non-ideal sensor limitations are addressed by S, through 
S3. 

The full _sweep and arc_sweep functions control S$; as described in the S; algo- 
rithm later in this section. It is assumed that a full 360° sweep of S; from any point x inside 
of W yields a correct partial world PW. It is also assumed, that given that PW,, is a correct 
partial world, an S; sweep of any “inferred” edge from a point x inside of PW, gives an 
edge list Q such that when Q is merged with PW,, a correct partial world PW,,,7 is derived. 

World W - The world W is a world as defined in Chapter VI. The world W has a 
finite number of holes and the boundary polygon P,, of W has a finite number of edges. All 
holes H), ..., Hy have a finite number of edges. All edges of W have finite length. An ar- 


bitrary parameter © exists such that no hole polygons are closer than 26 to the boundary 


polygon P, or to any other hole polygon. 


2. Example of Behavior 


To explain how the idealized automated cartography algorithm works, two exam- 
ples are presented. In the first example, R performs cartography by detecting the boundaries 
of a planar, closed polygonal world W. R is placed at any arbitrary point C inside of W, with 
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no knowledge of the configuration space geometry. R first sweeps sensor S; about a full 
circle. This is essentially an idealized visibility sweep that extracts a partial world PW 
[O’Rourke 87]. From the 360° edge data, R derives an edge list Q. In Figure 7.3(a), the 
shaded area is the sensor’s sweep volume. 

The partial world PW, has a boundary-type polygon P, and no hole polygons. P, 
has an edge list E;, a first edge e,, and a next function that determines the edge order as 


shown by Equations 7.1 and 7.2. 


PW, = (P, @) 71 


Py = (Ley, la, € 3s gs C5 Cgs C7» Cg, Cg, Cig} » Cy, NEXE) 712 


In PW; in Figure 7.3 (b), each of the consecutive pair of edges in E, = {e7, €2, €3, &4, es, 
6 €7, €g, €g, 79} Share a common vertex and the polygon forms a closed cycle such that 
the first (e,) and last edges (e;9) share a common vertex. In the polygon PW;°P,, the edges 


are classified as “real” or “inferred” based upon how they are derived from sensor input. 
“Real” edges represent the visible edges of W from C). “Inferred” edges are derived from 


discontinuity between the “real” edges. In the boundary polygon of the partial world 
PW,°P, in Figure 7.3 (b), edges e3 and eg are “inferred” and the rest of the edges are “real”. 
Therefore, an “inferred” edge represents S’s inability to sense beyond some obstructing 
portion of its environment. To perform the next step, R must chose a new optimum config- 
uration (or viewpoint) inside of PW; to perform the second sensor sweep. A depth first 
search (DFS) type algorithm selects the next “inferred” edge for investigation. In this case 
the “inferred” edge eg is chosen to be investigated next. R moves to position Cz near the 
center of edge eg and inside of PW with no dead reckoning error (idealized robot motion). 


Figure 7.3 (c) illustrates R’s choice for the second sweep configuration. 
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(b) First Partial Map of W: PW; 
PW) =(P,, ), 
P, = (Ey), €}, next), 


(a) Initial Sweep from 0 to 27% 







O71 = (gs Cbs Cer Cds Cf Cay Cs Cj» Cr Ces 


E} = fej, €9, €3, 4, &5, €6, 7, Cg, Cg, Crh 







e7 

(d) Second Partial Map of W: PW. 
_ PW) =(Po, ©), 
Po =(Ep, €7, next) 






€qg Vi 





(c) Second Sweep at edge eg from v; to v2 


02 = {€q, ep} 








Ep = (61, €2) €3, €4, €5s €61 €7, €8, €9, E10) 


[ Region swept by the sensor (aa free(PW) 


Figure 7.3 - S; Sensor Idealized Robot Cartography Example 
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(a) Third Sweep at edge e3fromv) tov, |  {b) Final Partial Map of W: PW; 
PW; =(P,, 2), 
Po =(E3, ey, next) 


Q3 = (eq, ep, &c, ed) 


E3 = (€ 112, €3, €4, €5, CG, 7, C8, Cg, €105 1} 12) 





[ Region swept by the sensor [| Region inside of the partial world 
Figure 7.4 - S; Sensor Idealized Robot Cartog-aphy Example 


For the second sensor sweep R moves to a point normal to the center point of the 


“inferred” edge ég. This point is called C>. R sweeps the sensor S; clockwise from vertex 
v , to vertex vz on edge ég in Figure 7.3 (c). The edge list for the partial world PW, is rep- 
resented by PW;*P,,*E. Then the derived “real” edge list Q2 = {eg, 2,} replaces the “‘in- 
ferred” edge eg in accordance with the Equation 7.3. Edges e7 and e, are collinear and are 
therefore combined into one new boundary polygon edge e7. The new edge e, gets renum- 
bered as ég and all of the edges in the boundary polygon get renumbered in consecutive 


fashion. 
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PW,¢P,°E, = PW,°P eE,-{eg} V {e,,e,} 13 
This process is accomplished by a “merge” algorithm. 

The second partial world PW3 is illustrated in Figure 7.3 (d). The only remaining 
“inferred” edge in PW? is e3. This edge is investigated next. R moves from point C2 to C3 
in Figure 7.4 (a). Again point C; is inside of PW>, normal to the center of “inferred” edge 
e3 and a distance o inside of PW>. R performs the next sensor sweep clockwise from vertex 
v), to vz on edge e; in Figure 7.4 (a). The extracted edge list is Q3; = {e,, ep, €,, €g}. The 
final partial world PW3 is derived by a merge function as shown by the Equation 7.4. 


PW,°¢P,°E, = PW,°P,°E,—{e3} U {€,, &), €,, ey} 7.4 


The remaining occluded edges of the world are revealed and the S, cartography 
is completed. Notice that the final world has twelve edges all “real”. When all edges in WP, 
are “real”, the algorithm returns the completed partial world PW,, = W as shown in Figure 
7.4 (b). 

A second automated cartography example serves to explain 5S; cartography on a 
world with holes W = {P,, H}. In Figure 7.5 (a), R is placed at point C;. R sweeps sensor 
5; ina full circle and extracts the edge list 0) = {€g, ep, €cs Cg» Cf, Cg, Cp &;}. The first partial 
world PW, = {P,, ©} is constructed in accordance with Equation 7.5. 

PW, °P,°E, = Q; 15 

H = @ since no hole yet exist in PW,°P,, as illustrated in Figure 7.5 (b). The al- 

gorithm selects the closest “inferred” edge in PW; for DFS. R next moves to point C2 and 


performs a second sweep on “inferred” edge eg of PW, as illustrated in Figure 7.6 (c). The 
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Region swept by the sensor 


(a) Initial Sweep 0 to 27 
Q; = {€q ep, lo eg Cf eg Ch» e} 





aa Region inside of the partial world 


Figure 7.5 - S; Sensor Idealized Robot Cartography 
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P| Region swept by the sensor 


(a) Second Sweep at edge ég: v; to v2 
Q2 = (eg &b: Cc Cg) 





[ Region inside of the partial world 
Figure 7.6 - 5; Sensor Idealized Robot Cartography Example 
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Region swept by the sensor 


eg v2 = ed 
(a) Third Sweep at edge e7: v; to v2 


O3 = L€q Cy €c a} 





ES | Region inside of the partial world 


Figure 7.7 - S; Sensor Idealized Robot Cartography Example 
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extracted edge list Q2 = {e,, ey, €,, eg} is merged with PW, in accordance with Equation 
7.6. 


PW,°P,°E, = PW,eP,°E,- {eg} U {ee Co Cg} 16 


The second partial world is PW2 = {P,, ©}. Notice that there are still no holes in 
PW? so H = © as illustrated in Figure 7.6 (b). There are two “inferred” edges eg and e7. In 
Figure 7.7 (a) the algorithm continues depth first search “inferred” edge e7 in PW2.R 
moves to point C; for the next sensor sweep. R sweeps the sensor S; from vertex v; to v2 
on “inferred” edge e7. This sweep extracts four new edges; é,, €,, €,, and €g. Notice that 
the “inferred” edges e, and e7 are included inside of the region sweep by S,. Therefore, 
these edges are removed. Further, the edges es, és, and €, bound a closed interior region. 
This region becomes a hole polygon as shown in Figure 7.7 (b) such that H 1= ({e, e7, 
€g}, €5, next}. The edge e, is “inferred” and included inside of PW), therefore it is discard- 
ed. In Figure 7.7 (a), edges e, and e2 from PW> are coincident so e, is discarded. Edges eg, 
€g, and é3 in Figure 7.7 (a) are also collinear and are combined to form the new boundary 
edge e3. Therefore, a new boundary polygon for PW; is derived by the Equation 7.7. 


PW, °¢P,e°E, = PW,°P,°E,- {eg} U {€,,€y €, egh 7 


The final partial world PW; is PW; = {P., {H,}} as illustrated in Figure 7.7 (b). 
Notice that the edges for the boundary polygon P, are directed clockwise and the edges of 


the hole polygon H; are directed counterclockwise. 


3. Algorithm 


This algorithm for idealized automated cartography is listed in Figure 7.8. At the 
top level, this algorithm is relatively straightforward. Initialization of variables occurs, an 
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S1_automated_cartography() 
{ 

World PW = (@, @); 

Real oa; 

Path_list PL = @, 

Edge List Q = @; 

Point C = (0, 0); 

Edge e; 

Vertex Vo, V); 


Q = full_sweep(C); 
PW.P,.E =Q:; 


while (not complete(PW)) 
{ 
e = DFS_edge(PW, C); 
Vo = first_vertex(e); 
v, = second_vertex(e); 
C = next_position(vo, Vj, 6) 
move_*o(C, PL, PW); 
Wo = VC, vo); 
Wi= YC. »v 13 
Q = arc_sweep(C, Vo, WV); 
PW = merge_edges(e, Q, PW); 
} /* end while */ 
return PW; 





Figure 7.8 - The S; Idealized Robot Cartography Algorithm 
initial full sweep from the starting position is made and then a “while loop” is executed each 
step of the world space exploration until the world PW is evaluated as complete. There are 
ten basic functions; full_sweep, arc_sweep, DFS_edge, Y, first_vertex, second_vertex, 


next_position, move_to, merge_edges, and complete. 


117 








full_sweep(C) 

Point C; 

{ 
Edge_List Q; 
Edge €first €lasp €: 


Sweep sensor S; in an arc clockwise 360°; 


Q = edge list derived from S;; 
Cfirst = Sirst_edge(Q); 
last = last_edge(Q); 
e = merge(efrsy Cass); 
O =O - first - Clase U &: 
return Q; 

} [* end full_sweep */ 





Figure 7.9 - The full_sweep algorithm 

The full_sweep(C) function takes R’s position C as input and returns an edge list 
Q of all edges of W visible from C. The full_sweep algorithm is listed in Figure 7.9. The 
first and last edges from the 360° sweep are merged together if necessary. 

The full_sweep function constructs a point visibility polygon [O’Rourke 87]. In 
Figure 7.10 (b), R performs a S; full sweep from 0 to 27 at point C). Edge list Q = {e), e2, 
€3, €4, €5, €g} is extracted. Then edges e; and eg are merged to form e;. R moves to the cen- 
terpoint of edge e3; and performs an arc_sweep on edge e3 Clockwise from v, to v7. Sensor 
5; detects five new edges beyond edge e3. The edges Q = {@,, €p, €¢, €g, &f &g} replace 
edge ¢3 in PW2°Po. 

The arc_sweep(C, Wo, W;) function sweeps the S; sensor clockwise at point C 
from the orientation y, to y, and returns an edge list Q. The algorithm for this function is 


listed in Figure 7.11. Q is a list of the newly found edges. The function returns an output of 
Q which has “real” edges derived from the visible portions of W’s edges, and “inferred” 
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Figure 7.10 - Example of S$; Sensor Sweep Operation 


edges that serve to bound occluded regions of W. The “inferred” edges are special edges 
that are constructed to connect the discontinuity between “real” edges. 

The DFS_edge(PW, C) function takes the current partial world PW and R’s cur- 
rent position C as input and returns the nearest edge suitable for sensor exploration. The 
function evaluates each existing “inferred” edge in PW and selects the best edge based upon 


minimizing the overall distance R must travel to complete the entire search. A depth first 
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arc_sweep(C, Wo, V1) 
Point C; 
Orientation Wo: Wi 7 


{ 
Edge _ List Q; 





Align S; sensor with orientation Yo; 
Sweep sensor S; in an arc clockwise from orientation Yo to YW; 
Q = edge list derived from S); 















return Q; 
} /* end arc_sweep */ 


Figure 7.11 - The arc_sweep Algorithm 
search strategy is used to guide exploration of complex worlds [Manber 89]. In other words, 
each branch of the world is investigated until an end is found to the branch or a hole is dis- 
covered. 

The ¥ function returns the orientation between any two input points from the first 
point to the second point. The first_vertex(e) function returns the first vertex in the edge e’s 
ordered pair of vertices. For example, for v = first_vertex(v,, v;, type) such that v = vo. 
Likewise, the second_vertex(e) function returns the second vertex of the edge e from its or- 
dered pair of vertices. For example, for v = second_vertex(v,, v;, type) such that v = v;. 
These functions are used to determine the vertices of any particular edge. 

The next_position(v,, v;, 6) function returns a point suitable for sensor investiga- 
tion of the edge represented by the pair of vertices (v,, v,). The O parameter is a constant 
that represents an arbitrary standoff distance for R to investigate the edge. A standoff dis- 
tance is necessary for S; to sweep the entire “inferred” edge with a non-zero sensor beam 
incidence angle. The point C is selected on a line constructed on the perpendicular bisector 
at a distance o from the edge. This concept is illustrated in Figure 7.12. 
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Figure 7.12 - The next_position function 


The move_to function moves R to the new configuration in preparation for the 
next sensor sweep. This function plans a collision free path for R from the current position 
C,, to the new position C,,, ; using the partial world model PW,, and PL. Then the function 
moves R to the new position using a series of one or more straight line path elements. R’s 
motion falls into two categories; (1) m.ovement to a visible “inferred” edge for an arc_- 
Sweep or, (2) backing tracking um the DFS tree to seek unexplored portions of the world 
space behind some “inferred” edge. This is a robot motion planning problem which has 
been well studied in the literature [Latombe 91] [Hwang 92]. 

The merge_edge function combines an edge list Q from an arc_sweep with the 
existing partial world PW. The algorithm for this function is listed in Figure 7.13. There are 
three input parameters; the “inferred” edge e under investigation, the derived edge list Q 
from the S; sweep, and the current partial world PW. This function combines the new edge 
list with the existing partial world PW,, and returns a new partial world PW,,,7. This func- 
tion checks the edge list Q for indications of new hole formation. This is indicated when 
one of PWeP,, “inferred” edges is within the region swept by the arc_sweep function. 

The complete function examines the existing partial world PW,, for “inferred” 


edges. If PW,, has no “inferred” edges, then complete returns TRUE; otherwise if PW,, has 
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merge_edges(e, OQ, PW) 
Edge e; 

Edge List Q; 
Partial_World PW; 

{ 








Edge List H; 






if (new_hole_detected(PW, Q)) 
{ 
H = extract_hole(PW, Q); 
PW-P,.E = PWP,,.E - H; 
PW.H = make_hole(H, e); 
} 
else 

PW-Po.E = PWPo.E-eUQ; 

return PW; 

} [* end merge_edges */ 













Figure 7.13 - The merge_edges Algorithm 
one or more “inferred” edges, then complete returns FALSE. A returned value of FALSE 
causes the overall algorithm to continue to run since all “inferred” edges must be resolved. 
When the complete function returns TRUE, this represents the terminating condition for the 
overall algorithm. 


4. Proof of Correctness and Termination 
The algorithm’s proof requires that the idealized robot R can map any arbitrary 
world with holes in a finite number of moves with R starting at any point C inside of W. 
The proof is by induction. The algorithm terminates after a finite number of steps since the 
W has a finite total edge length and each iteration of the algorithm reveals at least minimum 


length of new edges of W. 
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PROPOSITION 7.1: Let PW,, be a partial world obtained by the nth sweep in the 
world W of the idealized cartography algorithm listed in Figure 7.8. Then PW, is a correct 
partial world derived from W. 

Proof: 

Basis: For sensor sweep number n=/, and PW, is a correct partial world of W by 
the assumption that a full S, sweep from any point inside of W yields a correct partial world. 


Inductive Hypothesis: Assume that for sweep n=m the proposition is correct. 
Inductive Step: Then at n=m+J, R has a partial world PW,,,, ;. By the inductive 


hypothesis, the partial world PW,,, is correct. By assumption, the partial world PW,,, is 


also a correct partial world derived from W since PW,,,, ; is derived by merging PW,, with 


an arc sweep of some “inferred” edge in PW,,,. oO 


LEMMA 7.1 : Given any world W there exists a minimum “inferred” edge length 
[, 2 0 such that the length / of any “inferred” edge in a partial world PW,, for some n gen- 


erated by the S, algorithm in Figure 7.8 is greater than or equal to /, (J 2 /,). 





Figure 7.14 - Minimum Inferred Edge Length 


For instance in Figure 7.14, in the world W the minimum length “inferred” edge 


[, is shown. All other possible “inferred” edges have a length / longer than or equal to /,. 
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Proof: An “inferred” edge AB generated by the S, algorithm in Figure 7.11 has 
the following properties: 

(a) A is a vertex in W, 

(b) There is an edge e in W such that B is one, 

(c) Vertex A is not a point on edge e, 

(d) A and B are visible from each other. 

A sensor position C is assumed as in Figure 7.14. Consider the minimum length 
“inferred” edge AB. This is an “inferred” edge with one vertex fixed at A and the other ver- 
tex B on some edge of W. A minimum length /,, exists for any A (this is from the definition 
of polygons and a world in Chapter VI) and [4, 2 0. Therefore if we let |, = min ge w (lao) 
then /, is a positive constant for any given world W. Therefore, for all “inferred” edges in 
PW, the length is greater than or equal to /,. 0 

LEMMA 7.2 Given any sweep on an “inferred” edge e’ by the S; algorithm in 
Figure 7.8 the total length of the “real” edges derived from the sweep is greater than or 
equal to the length of edge e’. 


sweep from 
this point 





Figure 7.15 - An arc_sweep on “inferred” edge e” 


Proof: Let the function length(e) represent the length of a given edge e. Then a 


portion Ae of a “real” edge projected onto W corresponds to Ae’ in the “inferred” edge e’, 
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where Ae 2 Ae’. This concept is illustrated in Figure 7.15. By integrating both sides of the 
equation Ae 2 Ae’ over the length of e’ it is determined that length(e) > length(e’). 
Oo 
PROPOSITION 7.2: The execution of the S; cartography algorithm listed in Fig- 
ure 7.8 terminates. 
Proof: The total length /,, of the edges in the world W is finite. In the n arc 
sweep, at least part of the “real” edges of W with a total length of I, is found where 1, 2 I. 


Since J, is a positive constant by LEMMA 7.2, the execution of the S; cartography algo- 


1 
rithm terminates with at most (7) iterations. oO 
oO 


Conclusion: Since any PW,,,; produced by the m+/ step of the algorithm is a 


correct partial world of W and since the algorithm terminates after a finite number of itera- 


tions, the idealized sensor S; cartography algorithm gives a correct complete world derived 


from W. 


B. ALGORITHM FOR IDEALIZED SENSOR S, 
The S» sensor algorithm has one more non-ideal constraint placed upon it. This con- 
straint concerns the sensor beam incidence angle. S’s ability to extract edges from the 


world is limited by the incidence angle of its beam with respect to the incident edge of W. 
The S> cartography algorithm works only on orthogonal worlds because of the sensor beam 


incidence angle limitation. 


1. Assumptions for the Sz Algorithm 


Idealized Sensor S2 - The sensor Sz moves about in W attached to the center point 
of R. This sensor S. is a modified, ray-tracing sensor with infinite range. S> differs from S; 
in that S> returns only the portion of an edge that is both visible and within the incident an- 


gle limits of to of normal to the target surface as illustrated in Figure 7.16. For this reason, 
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Free part of W 


5S Beam 


Figure 7.16 - Sz) Sensor Limited Incidence Angle Capability 


R performs S» cartography primarily by translational scanning. As with the S, algorithm, 
Sz connects the discontinuities between edges on W by connecting the “inferred” edges. 
These “inferred” edges bound the occluded regions inside of W that are not visible from R. 

Suppose R is placed at any point inside of W, then assume any a translational scan 
orthogonal to world W that stops at a distance o from any edge of W yields a correct partial 
world PW of W. Also assume given a translational scan entirely inside of a partial world 
PW, derived from W yields a correct partial world PW,,,. These assumptions are charac- 
teristic of the idealized sensor Sp. 

World W - The world W = {P,, H} is an orthogonal world with holes as defined 
in Chapter VI. The world W has a finite number of holes h and the boundary polygon P,, of 
W has a finite number of edges. All holes H = H), ..., H;, have a finite number of edges. 


All edges of W have finite length. The world is restricted to an orthogonal world due to the 
limited incidence angle capability of Sz. The minimum distance between the boundary 
polygon P,, and any hole polygon H; € H must be greater than o. 


2. Example of Behavior 
An example of sensor Sz cartography is presented starting in Figure 7.17. In Fig- 
ure 7.17 (a), R is placed at any arbitrary starting point C. R sweeps the S> about a full circle 


and extracts the edge list Q consisting of the four edges; eg, ep, €,, and ey. In any given 
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Figure 7.17 - Sz Sensor Idealized Robot Cartography 


127 


world at least four edges are extracted by this full sweep. More than four edges are possible. 
These edges must meet the incidence angle criteria of ta to be visible as described above. 
The orientation of R’s first translational scan is based upon the edge list Q such that R 
moves orthogonal with the world W. The position of the edges derived from the initial ro- 
tational sensor scan are used to determine the path for the first translational scan. The algo- 
rithm selects the edge furthest away in edge list Q to move toward for the first translational 


scan. In this case edge e, is the furthest C,. Then R moves from C, to C2 using a straight 


line path. The first translational scan is shown in Figure 7.17 (b). Notice that a hole polygon 


H, was initially undetected by the S sensor sweep due to the incidence angle limitations. 
Also notice that 2 partial world PW; extracted by the first translational scan is an orthog- 


onal world. 


The S> algorithm uses the resulting partial world to determine the path for the next 


translational scan. Since the right hand side of the first partial world is a long “inferred” 


edge e2 in Figure 7.17 (b), R performs the second translational scan from configuration C2 
to C3. The second translational scan reveals the right hand side of the boundary polygon of 
the world and the right hand side of the hole polygon H;. Using the same reasoning process, 
R continues with the third translational scan from C3 to C4. More of the boundary and hole 
polygon edges are revealed. In Figure 7.17 (a), the final translational scan from C4 to Cs 


completes the map of the world. Notice that “inferred” edges near the four comers of the 
boundary polygon are incorporated in the “real” edges. The “inferred” edges in the interior 
corners of P,, are connected so that R maintains a standoff distance o from any “real” edge 


in W. 


3. Algorithm 
The algorithm for the Sz sensor cartography is listed in Figure 7.19. At the top lev- 
el the algorithm first initializes all variables. Then a rotational sweep is performed to deter- 


mine the visible edges of W. The algorithm then enters a “while loop” and iterates until the 
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P= {Es, e}, next} : ?p, = {E. ei; next) 
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Figure 7.18 - Sz Sensor Idealized Robot Cartography 


partial map PW is evaluated as completed. There are six basic functions; full_sweep, find_- 
orthogonal _orientation, translational_scan, merge, next_position, and complete. 

The full_sweep function sweeps R’s Sz sensor clockwise from 0 to 27 and extracts 
an edge list E. This edge list represents all “real”, detectable edges from R’s position. In the 
example, the “real” extracted edge list in Figure 7.17 (a) is Q = {@,, ep, €,, €g}. This func- 
tion always extracts at least four “real” edges since S> has infinite range and W is an orthog- 
onal world. More than four edges are also possible depending on R’s initial position and the 
geometry of the world. 

The find_orthogonal_orientation function aligns R’s internal coordinate system 
such that it is orthogonal to the edges in the input list Q. This function examines each edge 
in the edge list Q and returns a point C such that R moves orthogonal to the edges in Q when 
it moves on its first translational scan. This concept is illustrated in Figure 7.20. Edge ey, is 


the furthest edge, but R cannot move to e, on a single straight line path orthogonal to the 
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S2_automated_cartography() 

{ 
World PW = (@, ©); 
Edge _listQ =; 
Configuration C = (0, 0, 0); 


Q = full_sweep(C); 

C = find_orthogonal_orientation(Q); 
while (not complete(PW)) 

{ 


Q = translational _scan(C, PW); 
merge(Q, PW); 
C = next_configuration(PW, C) 
} /* end while */ 
return PW; 





Figure 7.19 - Sz Sensor Idealized Robot Cartography Algorithm 


edges in W. Therefore, the function picks the next furthest edge e, and calculates a point C 
at a distance 0 from e, as shown in Figure 7.20. 

The translational_scan is an analog of the S; move_to function except that S$, op- 
erates while R moves translationally. The reduced capability of the Sz sensor requires R to 
use a translational scanning technique to find edges in W. The translation_scan function 
moves R on a straight line path from its current position to the C,, position. The path chosen 


is always orthogonal to W and runs parallel to one or more of W’s edges. The translation_- 
scan function operates while R is moving translationally along a straight line path element. 


During the translational scan, the Sz sensor scans perpendicular to R’s direction of travel 


on both the left and right sides. The translational_scan function returns an edge list Q. 
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first scan path element 


Figure 7.20 - The find_orthogonal_orientation function 
The merge function combines an edge list Q extracted from a single translational 


scan with the existing partial world PW,, to form PW,,,7. The merge function must recog- 


nize and adapt to holes found in the world. In the example in Figure 7.17 (d), the merge 
function recognizes a hole polygon during the translational scan from C3 to C4. The “real” 
edges é@5, 5, €7, and ég and the next function for the polygon are modified to change the 
directionality of the hole edges such that H, = {{és5, €g, €7, eg}, és, next}. 

The next_position function takes the existing partial world PW and R’s position 
C,, aS input parameters and returns the next point C,,, for R to move to for translational 


scanning. This function examines all imaginary edges in the current partial world and se- 
lects the imaginary edge closest to R’s current position. Then R calculates a point inside of 
PW, such that R will move parallel to this imaginary edge an inside of PW,. If there are no 


imaginary edges left in PW,, then the next_position function returns R’s current position. 
The complete function evaluates PW,,. If any “inferred” edges longer than o re- 
main in PW, then PW is evaluated as incomplete and FALSE is returned. If PW = (O, ©), 
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then PW is also evaluated as incomplete and FALSE is returned. Otherwise, if all “inferred” 
edges in PW are shorter than o, then PW is evaluated as completed and TRUE is returned. 


4. Proof of Correctness and Termination 


PROPOSITION 7.3: Let PW,, be a partial world obtained by the n* translational 
scan of the world W of the S> idealized cartography algorithm listed in Figure 7.19. Then 
PW, is a correct partial world derived from W. 

Proof: 

Basis: For translational scan number n=/, PW, is a correct partial world of W by 
the assumption that any translational scan in W yields a correct partial world. 

Inductive Hypothesis: Assume for translational scan n=m the proposition is cor- 
rect. 

Inductive Step: Then at n=m+1, R has a partial world PW,,,). By the inductive 
hypothesis, the partial world PW,,, is correct. Since PW, is derived by merging the edges 


from a translational scan inside of PW,,,, the partial world PW,,,,; is a correct partial world 


derived from W. Oo 
Given an arbitrary orthogonal world aligned in general position list the x-coordi- 


nates of all the vertical edges listed in ascending order is x, x2, ..., Xp and the y-coordinates 
of all of the horizontal edges listed in ascending order is y), y2, ..-, Yg- The minimum length 
translational scan length is defined by Equation 7.8. 


1 Smin (ming ¢ ig ps1 Oi41 —%) 0 MIM cjcq41 Oj41-¥)) 78 


LEMMA 7.3 : Given any orthogonal world W there exists a minimum translation- 


al scan length J; 2 0 such that the length / of any translational scan in the world W for some 


n generated by the S2 algorithm in Figure 7.19 is greater than or equal to /; (/ 2 /;). 
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Figure 7.21 - A Minimum Length Translational Scan /; 


Proof: Since translational scans are either horizontal or vertical and since /; is the 
minimum distance between any two successive edges perpendicular to R’s path, the mini- 
mum length scan is /; as illustrated in Figure 7.21. Oo 


LEMMA 7.4 : For any translational scan by the S> algorithm in Figure 7.19 the 


total areas swept by the translational scan is greater than or equal to / 7. 
Proof. The minimum length of any translational scan in a world W is 1; by LEM- 
MA 7.3. By the same token the minimum width of the area scanned is /;. Therefore the min- 
imum region swept by any translational scan must be greater than or equal to / Fg a 
PROPOSITION 7.4: The execution of the Sz idealized automated cartography al- 


gorithm listed in Figure 7.19 terminates. 


Proof: Let A represent the finite total area of the region enclosed by a world W. 


By LEMMA 7.2 the minimum area swept by any S> translational scan is / 7? . Therefore the 


execution of the Sz cartography algorithm terminates with at most (7) itera- 
1 


tions. QO 
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Conclusion: Since any PW,,,, produced by the m+J step of the algorithm is a 
correct partial world of W by Proposition 7.3 and since the algorithm terminates after a fi- 
nite number of iterations by Proposition 7.4, the idealized sensor Sz cartography algorithm 


returns a complete correct partial world derived from W. 


C. ALGORITHM FOR IDEALIZED SENSOR S$; 

The S3 sensor is an idealized sensor with capabilities that are less ideal than S, or S3. 
For this sensor, the range is limited to arbitrary value B. As with the S> algorithm this algo- 
rithm operates only on orthogonal worlds because S; has the same incidence angle limita- 


tion as Sp. 


1, Assumptions for the S; Algorithm 
Idealized Sensor S; - The sensor S; moves about in W attached to R. This perfect 
sensor 53 is a modified, ray-tracing sensor with finite range. S; differs from S; in that S; 
returns only the portions of an edge that are incident ta with the sensor’s ray. S; differs 
from S; and S> in that it is capable of limited range B. Therefore, S; operates primarily by 
translational scanning. Further, S; connects the discontinuities between edges on W by “‘in- 


ferred” edges and uses “inferred” edges to bound a region out of sensor range. These “in- 
ferred” edges bound the occluded regions inside of W that are not visible from the scan po- 


sition. During translational scanning, 53 scans both sides of R’s path perpendicular to R’s 
direction of travel. S; also monitors forward range to any obstacle. In Figure 7.22, R per- 
forms a translational scan from C; to C>. The sensor S$; has a side scanning beam on either 


side of R. In Figure 7.22, the object on R’s right hand side is extracted as a “real” edge since 
its range is within the maximum range B from R. The left hand side object is beyond S3’s 


range, so $3 constructs an “inferred” edge parallel to R’s translational scan path. R stops at 
the point C2 at a distance o from the obstruction. The imaginary edges on either side of R 


are extended to include this barrier. The distance 6 is an arbitrary, small standoff distance. 
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Figure 7.22 - - S; Translational Scanning 


Suppose R is placed at any point inside of W, then assume any a translational scan 
orthogonal to world W that stops at a distance o from W yields a correct partial world PW 
of W. Also assume given that a partial world PW,, is a correct partial world derived from W, 


a translational scan that moves R parallel to any existing “inferred” edge yields a correct 


partial world PW,,, 7. These assumptions are characteristic of the idealized sensor S3. 
World W - The world W = {P,, H)} is an orthogonal world as defined in Chapter 

VI. The world W has a finite number of holes and the boundary polygon P,, of W has a finite 

number of edges. All holes H = H;, ..., H, have a finite number of edges. All edges of W 


have finite length. 


2. Example of Behavior 
The S3 algorithm behavior is illustrated in Figure 7.23. R is initially placed at 
point C;. An initial sensor sweep extracts an edge list Q with two edges, e, and e,, from 
the world. Notice that less information is obtained by a sensor sweep due to S3’s limited 


range and incidence angle capability as shown in Figure 7.23 (a). Based upon these two ini- 
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Figure 7.23 - The S; Cartography Example 
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tial edges, R moves parallel to e, from point C; to C2. This is the first translational scan 
represented by the white area in Figure 7.23 (b). Notice that the hole polygon H, is out of 
range of sensor S;. The direction for the second translational scan is chosen based upon S3’s 
sensor input to R at point C>. R detects an obstruction forward and to the left. Therefore, it 
turns right 90° for the second scan. The second translational scan moves R from point Cz 
to C3; as illustrated in Figure 7.23 (c). R stops moving when a barrier is sensed forward at a 
distance o. The partial world PW; is now composed of two “real” edges and several “‘in- 


ferred” edges. These “inferred” edges require resolution by translational scan. R continues 


” 


translational scanning by moving from C3 to C4 as illustrated in Figure 7.23 (d). Two “r 
edges are extracted by this scan, one from the boundary polygon (e3) and one on the hole 
polygon H; (e7). R next moves from C4 to Cs for the fourth translational scan. This motion 
is determined by the proximity of the closest inferred edge. Once Cs is reached, several in- 
ferred edges remain inside of the boundary polygon. R moves from C; to Cg to C7 to com- 


plete the map as illustrated in Figure 7.23 (f). 


3. Algorithm 

The algorithm for the S3 sensor cartography is listed in Figure 7.24. At the top lev- 
el the algorithm first initializes all variables. Then a rotational sweep is performed to deter- 
mine the visible edges of W. The algorithm then enters a “while loop” and iterates until the 
partial map PW, is evaluated as completed. There are six basic functions; full_sweep, fin- 
d_orthogonal_orientation, next_orientation, merge, translation_scan, and complete. 

The full_ sweep function sweeps R’s S3 sensor through 360° for one circular scan 
and returns an edge list Q. All portions of W’s “real” edges in sensor range that are visible, 
within range f and that have the correct incidence angle @ are extracted. R’s internal coor- 
dinate system is aligned to the visible edge of Q. In this way R aligns its coordinate system 
orthogonal to W. 
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S3_automated_cartography() 
{ 
World PW = (©, @); 
Edge _listQ = @; 
Point C = (0, 0); 
Orientation Y; 


Q = full_sweep(C); 

'Y = find_orthogonal_orientation(Q); 
while (not complete(PW)) 

{ 


Q = translational _scan(y, PW); 
merge(Q, PW); 
'Y = next_orientation(PW) 

} /* end while */ 

return PW; 





Figure 7.24 - S; Sensor Idealized Robot Cartography Algorithm 


The find_orthogonal_orientation function takes the edge list Q as an input and ro- 
tates R to an orientation suitable for translational scanning. This function determines which 
of the visible edges in the edge list is best for guiding R’s first translational scan. The fin- 
d_orthogonal_orientation function is somewhat complicated by all of the possible config- 
urations of edges extracted by the sweep function. The algorithm for the find_orthogonal_- 
orientation function is shown in Figure 7.25. There are six important cases as illustrated in 
Figure 7.26. If the input edge list Q is NULL, then there were no visible edges after R’s first 
Stationary, circular sweep. In this case R performs a series of circle searches to locate any 
edges that may be nearby as shown in Figure 7.26 (a). The circles used for the edge search 
start out small and then grow progressively larger until an edge is located. The first edge 
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find_orthogonal_orientation(Q) 


edge_list Q; 
{ 
ifQ = © then 
While (Q = ©) 


Q = circle_for_edges(); 
Y= rotate_parallel_to_one_edge(Q); 
else if (num_edges(Q) = 1) 
‘YY = rotate_parallel_to_one_edge(Q); 
else if (num_edges(Q) = 2 and edges_parallel(Q)) 
Y = rotate_parallel_to_two_edges(Q); 
else if (num_edges(Q) = 2 and edges_perpendicular(Q)) 
Y = rotate_to_point_out_of_corner(Q); 
else if (num_edges(Q) = 3) 
‘Y = rotate_towards_open_space(Q) 
else if (num_edges(Q) = 4) 
YY = rotate_toward_furthest_edge(Q); 
return ¥ 


Figure 7.25 The find_orthogonal_orientation algorithm 


found is used to determine the path element for the first translational scan using the ro- 
tate_parallel _to_one_edge function. 
The num_edges function processes the edge list Q and determines how many sep- 


arate edges are visible. This function does not count repeat edges, i.e. edges with the same 





orientation and position twice. When the function num_edges returns one, meaning that 
only one edge is visible and in range, R simply uses this single edge to guide the first trans- 
lational scan as shown in Figure 7.26 (b). R rotates to align itself parallel to the edge’s ori- 
entation and uses the extracted edge’s position to calculate a suitable path element for the 


first translational scan. 


V/ 


(a) No segments - circle (b) One segment - rotate to align 
parallel 


[Va] 


(c) Two parallel segments - rotate to (d) Two perpendicular edges - 
align parallel rotate to leave the corner 


ja 


(e) Three edges - rotate to point (f) Four edges - rotate towards the 
towards open space furthest edge 





Figure 7.26 The find_orthogonal_orientation cases 
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The edges_parallel function examines the edge list Q and returns TRUE if all of 
the included edges are parallel and FALSE otherwise. If the nwn_edges function returns 
two and the edges_parallel function returns TRUE, then R rotates parallel to the edges in 
Q and uses the geometry of these edges to determine the best path element for the next 
translational scan. This case is as shown in Figure 7.26 (c). This behavior is appropriate for 
when R is starts in a hallway that is less than 2B wide. 

In Figure 7.26 (d), R uses the position of the two perpendicular edges to rotate to- 
wards open space. This is appropriate when R starts near a corner in its world space. In Fig- 
ure 7.26 (e) R uses the three edges to rotate towards open space and determine the best first 
scan path element for translational scanning. In Figure 7.26 (f), four edges are detected, in 
this case R simply rotates to point towards the edge that is furthest away. This behavior al- 
lows the first translational scan to map as much open space as possible. 

The translational_scan function takes an orientation ‘Y as an input and moves R 
along a path element with this orientation. Only four values of the input angle are possible 
0, m/2, m, or -%/2. The reduced capability of the S; sensor requires R to use a translational 


scanning technique to find edges in W. The translation_scan function moves R on a straight 


line path from its current position C,, to the C,,,, position. The path chosen is always or- 


thogonal to W based upon sensor input and therefore runs parallel to one or more of W’s 
edges. 

The merge function merges the extracted edge list Q with the current partial world 
PW,, to obtain a new partial world PW,,,;. The function recognizes hole emergence in 
PW,,4) by examining the existing edges in PW,. 

The next_orientation function takes the existing partial world PW as input and de- 
termines the path required to reach the next appropriate translational scan. This algorithm 
steers R parallel to the appropriate “inferred” edge in PW using a depth first search strategy. 

The complete function evaluates the partial world PW. If PW = (©, ©) or PW con- 


tains at least one “inferred” edge longer than o, the complete function returns FALSE. If all 








“inferred” edges in PW are shorter than 6, then complete returns TRUE. If PWeP, is com- 
plete and fully scanned and PWeH = @ then PW is evaluated as complete. 


4. Proof of Correctness and Termination 


PROPOSITION 7.5: Let PW,, be a partial world obtained by the n'* translational 
scan of the world W of the S; idealized cartography algorithm listed in Figure 7.24. Then 
PW, is a correct partial world derived from W. 

Proof: 

Basis: For sensor sweep number n=], PW, is a correct partial world of W by the 
S3 assumption that any translational scan yields a correct partial world. 

Inductive Hypothesis: Assume for translational scan n=m the proposition is cor- 
rect. 


Inductive Step: Then at n=m+1, R has a partial world PW,,,,). By the inductive 


hypothesis, the partial world PW,, is correct. By the S; assumptions, the partial world 


PW,+ iS a Correct partial world derived from W. 0 
There exists a minimum width (/2 > 0) for any translational scan. The value of /2 


is a positive constant less than 2B for any given world W. Given an arbitrary orthogonal 
world in general position, the x coordinates of all of the vertical edges in ascending order 


is Xj, X2, -.., Xp and list the y-coordinates of all of the horizontal edges in ascending order 
Vy Yar v001 Yge The minimum width of a translational scan (/2) is defined by Equation 7.9. 
ly = min (ming < icy g 1 Aja ~ 2, ~ 2B)» miny cjg 941 jn ~9j7-2MB)) 7g 


where (x; 7 - x; - 2mB) > 0 and (y;,.; - y; - 2mB) > 0 for non-negative integer values 
of m. 


LEMMA 7.5 : The minimum area swept by an S$; translational scan is [)x/>. 
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Proof: Since LEMMA 7.2 states that the minimum length of a translational scan 
is I, for a given world W and since the minimum width of an S; translational scan is defined 
as Jy, the minimum areas swept by a single translational scan is /)xJ9. oO 

PROPOSITION 7.6: The execution of the idealized automated cartography algo- 
rithm listed in Figure 7.24 terminates. 

Proof: Let A represent the finite total area of the region enclosed by the world W. 
By LEMMA 7.5 the minimum area swept by any translational scan is /)x/>. The execution 


A 


of the S3 cartography algorithm in Figure 7.24 terminates with at most ( cx 
aay 





) transla- 


tional scans. o 


Conclusion: Since any PW,,4 produced by the m+/ step of the S; algorithm is 
a correct partial map of W and since the algorithm terminates after a finite number of iter- 
ations, the idealized sensor S; cartography algorithm gives a correct complete world de- 


rived from W. 


D. SUMMARY 

A series of three algorithms for automated cartography for an idealized robot with pro- 
gressively less idealized sensors are presented. Examples provide an illustration of the idea 
behind each of the algorithms. The S; sensor is capable of cartography of non-orthogonal 
worlds since this sensor can extract information from the world regardless of incident an- 
gle. The Sz sensor is an infinite range sensor but with limited incidence angle capability. 
This limited incidence angle capability restricts the Sz sensor to cartography of only orthog- 
onal worlds. The $3 has both limited range and limited incidence angle capability. This sen- 
sor is most like the “real” sensors on Yamabico. Therefore, the 5; algorithm provides the 
basis for the “real” cartography algorithm developed in Chapter VIII. 

All component parts of the three algorithms are explained. The algorithms are each 


proven by induction on the number of iterations. The total number of “real” edges in the 
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partial world grows with each iteration of the algorithm until the world W is completely 
mapped. The odometry error incurred by robot motion and the “real” robot’s non-holonom- 
ic motion are the non-idealized constraints imposed by R. Finite sensor range, sensor noise, 
and sensor return limited by sensor beam incidence angle impose “real” constraints on the 


idealized sensor. These “real” world constraints are investigated in Chapter VIII. 
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Vill. AUTOMATED CARTOGRAPHY BY YAMABICO-11 


In Chapter II, numerous approaches to robot navigation and automated cartography 
were presented. None of these systems achieved simultaneous robot localization and car- 
tography. Both localization and cartography must be performed at the same time for a robot 
to build an accurate, spatially-consistent map of an interior space. This system supports si- 
multaneous robot cartography and localization since Yamabico can perform dead reckon- 
ing error corrections during translational scans orthogonal to the robot’s world space. 

In Chapter VII a series of algorithms for idealized automated cartography are present- 
ed. These algorithms have no practical utility in the real world since physical principles pre- 
clude their implementation. Idealized robot cartography provides a theoretical springboard 
for the development of the real sensor automated cartography on Yamabico-11. This chap- 
ter describes the physical characteristics of a real robot and a real sensor that cause the de- 
viation from the idealized case. The real sensor cartography algorithm is explained in detail 
in this chapter. The experimental results of real sensor cartography appear in Chapter IX. 

Important differences exist between the idealized robot-idealized sensor pair and the 
real robot with an array of real sensors. In Chapter VII, the idealized robot R moves about 
in the world space without incurring any dead reckoning error. Therefore, R always has per- 
fect knowledge of its current configuration. Yamabico, on the other hand, cannot move an 
appreciable distance without accumulating some dead reckoning error. The reasons for this 
fact are explained in Chapter V. Yamabico’s dead reckoning error increases as a function of 
the total distance traveled since the last odometry reset. Other factors affect the rate of dead 
reckoning error buildup per unit distance traveled; they are floor smoothness (related to in- 
tegrated distance traveled), amount of turning the robot does per unit distance (related to 
wheel slip), and the robot’s speed. 

Concerning sensor capabilities, the idealized sensors in Chapter VII have progressive- 


ly more real limitations. The practical range of a real active sensor is limited by spreading 
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losses and by sampling rate as discussed in Chapter II. For Yamabico’s ultrasonic range 
finders, the practical limit on the range is about four meters for target detection and about 
two meters for reliable edge extraction. As discussed in Chapter II, there is a trade-off be- 
tween sonar range gate and the data rate for range returns. 

The S; sensor extracted edges from the world space regardless of sensor beam inci- 
dence angle. In practice, an active emitter sensor has a limit on incidence angle for range 
returns. Yamabico’s ultrasonic range finders have a incidence angle limitation that varies 
with the target distance and target material. For a close, strongly reflective target sonar is 
most like the idealized sensor S;. 

All idealized sensors presented in Chapter VII give returns independent of target ma- 
terial and range. None of the idealized sensors in Chapter VII suffered from specular reflec- 
tions. With idealized sensors, only the primary sensor beam was reflected by the target sur- 
face. For the real sensor, secondary or so-called “specular” reflections further complicate 
the analysis of the sonar data [Leonard 91]. The first reflection of the sensor beam is not 
always the range returned by a real sonar. This is particularly true when the sonar is aimed 
into a concave corner. The cartography algorithm presented in this chapter works within the 
physical limitations of the real robot and the real sensor. 


A. REPRESENTATION OF THE WORLD 

This section examines the physical limitations of Yamabico’s locomotion system and 
sensors that cause them to be non-idealized. This explanation is important to bridge the gap 
between the real and idealized robot. 

The data structure used for the map representation must efficiently store the current 2D 
map of the world in a compact data structure. This requirement is based upon the limited 
space in Yamabico’s on board memory. Additionally, a smaller data structure can be more 
quickly searched. The data structure chosen must be a dynamic-type data structure since 
the number of features is not known beforehand. Grid-based map storage schemes use a 


fixed size storage area that cannot be grown dynamically. The type of structure allows for 
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the inclusion of additional edges as Yamabico builds a map of the world. Features can be 
added to the map until Yamabico runs out of memory. Yamabico-11 has five megabytes of 
main memory with a planned upgrade to 16 megabytes. Therefore, a map with a consider- 
able number of features can be stored in Yamabico’s main memory since each partial world 
edge requires only 32 bytes of storage space. If a grid-based scheme with a 10 cm grid size 
is used a byte of memory is used to store each square, 32 bytes could only store a region 


0.32 m2. 

Since the map is used for navigation, it must be quickly processed as a part of the ro- 
bot’s spatial reasoning tasks. A detailed map requiring a small amount of storage space may 
be quickly processed using Yamabico’s on board computing resources. One example is the 
determination of the next scan path for translational scanning. Also a quick search of the 
current partial world is frequently required to match newly detected features against exist- 
ing features to determine if the new feature is a repeat of a previously detected surface or 
the first time this particular object has been detected. This gives the system the capability 
to recognize the difference between a new line segment and one that is already part of the 
map. 

Lastly, the map must be transferred back to the host computer to allow for human in- 
spection. This is required for debugging and improving the system. This feature also allows 
the map to be saved and used later on a subsequent robot missions. 

The partial world data structure is the means of map representation in this dissertation. 
The concepts were introduced in Chapter VI. The partial world data structure is implement- 
ed as a doubly linked list of typed edges derived from sonar data and refined by repeated 
robot scans. Edges are typed as either “real” or “inferred” depending on how they are de- 
rived from sonar data. “Real” edges are derived from line segments extracted from sonar 
linear fitting data. “Inferred” line segments are constructed based upon the geometry of the 


existing “real” line segments in the PW. They serve to bound the unexplored area of the 


world. 











1. Real World Issues 

A fundamental problem with the Yamabico’s locomotion system lies with regard 
to its dead reckoning capability. No matter how finely tuned the wheel encoders and dead 
reckoning algorithm, the robot odometry estimate drifts as a function of the distance trav- 
eled. The mathematical principles behind robot odometry are examined in Chapter V. A 
real robot must use sensor input to correct odometry drift. This is necessary since all sensor 
input for cartography is recorded with reference to the robot’s current odometry configura- 
tion. Therefore, the map built by Yamabico is only as accurate as the estimate of its actual 
configuration in the world space. 

In order to explore its entire world space, Yamabico must move to correctly po- 
sition its sonar array for translational scanning. This motion incurs some dead reckoning 
error. Yamabico uses a straight line wall assumption with regard to its world space. This 
assumption allows some dead reckoning errors to be corrected during translational scan- 
ning. Using sonar range values, Yamabico’s orientation and distance to a long, straight 
world edge can be corrected. The details of odometry correction during translational scan- 
ning are discussed in section B of this chapter and the experimental results are described in 
Chapter IX. 

Given a partial world and sensor input, a robot can match features from the map 
to sensor input to correct odometry error. Extensive background research on robot localiza- 
tion is examined in Chapter II. In every case previously described, the world map used for 
localization was derived from a priori input. The automated cartography algorithm uses 
only a sensor derived partial world and indoor building heuristics (orthogonal world as- 
sumption) to correct odometry error. Specifically, Yamabico takes advantage of straight 
walls to correct some dead reckoning error. 

Real sensor limitations also force real robot cartography to deviate from idealized 
cartography. The sensor chosen for thi« work is the ultrasonic range finder. The ultrasonic 
range finder is widely used in mobiic robots and is readily available. Additionally, the au- 


thor’s work builds upon a considerable library of sonar processing functions already in- 
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cluded in the MML system [Sherfey 91]. The dissertation test bed robot, Yamabico-11, em- 
ploys 12 ultrasonic rage-finders as its primary sensor as described in Chapter III. The dif- 


ferences between the abstract idealized sensor described in Chapter VII and the real sensor 
are described here. 

Idealized sensor S, and S4 are capable of providing data regardless of target range. 
This is not true for the real sensor. All real active sensors have some practical range limi- 
tations for a variety of physical reasons. For ultrasonic range finders this range limitation 
in air is typically about four meters. The range limit is due primarily to spreading losses and 
attenuation by the propagating medium. This range limit suggests that inferred edges be- 
come more important for real robot cartography since the robot’s sensor cannot scan as 
much area as the idealized sensor. 

The idealized sensor S; provides the robot with input data regardless of the sensor 
beam incidence angle. This is not true for real ultrasonic range finders as discussed in Chap- 
ter II. In Figure 3.3, the real sensor incidence angle for a valid range return varies with tar- 
get range. In most cases, the sensor axis must be nearly normal to the target surface in order 
to obtain a valid range return. The real sensor is more ideal at closer range because spread- 
ing losses and attenuation are smaller. The incidence angle of « =+10 degrees from the nor- 
mal to the surface typically gives a valid retum when range is between Bpin = 9.3 to Brax 
= 200.0 centimeters. The real sonar has a minimum as well as a maximum range value. The 
minimum range of the sensor was not considered at all for the idealized sensors in Chapter 
Vil. The minimum range value of Yamabico’s sonar system is explained in Chapter III. 

The final limitation of the real sensor is specular reflections. In many cases, sonar 
returns the range to the second or third reflecting surface as the range value. This is caused 
by the fact that a smooth target surface acts as an acoustic mirror. These specularities are 
typically non-orthogonal line segments in the case of Yamabico-11. Several attempts to ful- 
ly model specular reflection of sonar range finders have been made [Leonard 91][Kuc 91], 
but a complete model does not yet exist. Specular returns typically form short line segments 
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that are not orthogonal to Yamabico’s world space. The automated cartography algorithm 
filters out these bad segments as Yamabico builds a map of its world space. 


2. Definitions 

World - The definitions of vertex, edge, polygon, orthogonal polygon, world and 
partial world are unchanged from the definitions given in Chapter VI. A world is a portion 
of a single floor of an office building. 

Yamabico - The robot is the Yamabico-11 mobile robot as described in Chapter 
If]. Yamabico is a non-holonomic robot capable of stationary rotation and translational mo- 
tion. Robot rotation may be clockwise or counterclockwise. The translational motion is the 
means of path tracking of straight line segments, circular arcs, cubic spirals and parabolic 
line segments. Only straight line and circular arc path elements are used for Yamabico’s 
cartography algorithm. 

Sensors - The robot’s sonar array S is treated as an abstract entity in this chapter. 
S is capable of range-finding forward, extracting edges while rotating, and translational 
side scanning with edge extraction. The Yamabico has an array 12 ultrasonic range finders 
as shown in Figure 3.2. For the purposes of cartography four sensors are used. Two forward 


looking sensors (numbers 0 and 3) and one on each side (numbers 4 and 7). 
B. THE ALGORITHM 


1. Assumptions 
Real Robot - The real mobile robot Yamabico-11 measures 54 centimeters square 
and is 95 centimeters high. It is a power-wheel-drive robot capable of translational motion 
as well as stationary rotational motion. Yamabico has a nominal velocity of +30.0 centime- 


ters per second but is capable of any velocity between -60.0 and +60.0 centimeters per sec- 


ond. 
Sensors - These sensors are assumed to have an effective range Brin = 9.3 to Bmax 


= 200.0 centimeters and provide a return if the sonar beam incidence angle is o = +10° of 
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the normal to the target surface. This assumption is based upon the experimental data plot- 


ted in Figure 3.3. The sonar sensors extract data in the form of line segments representing 
surfaces of the world W. Line segment data closely orthogonal to Yamabico’s current par- 
tial world that are derived from greater than 10 sonar returns are used for input to cartogra- 
phy. All other line segments data is discarded as specular reflections. The value of o = 75.0 
cm is used since 6 = 54.0 + 9.3 + 9.3 = 75.0 cm. The value of o represents the minimum 
opening through which Yamabico can safely navigate. All sonar returns at a range greater 
than 200.0 cm are discarded since S is only effectively finds edges up to this range. 

World - The world W = {Po, H} is an orthogonal world as defined in Chapter VI. 
The world has a finite number of holes. The boundary polygon P, and all holes H), ..., Hp 
have a finite number of edges. All edges in W have finite length. All world surfaces have a 
target strength greater than 50% at 2.3 meters range in accordance with Figure 3.6. 


2. Example of Behavior 

A right wall following method of cartography with left turns at obstacles is used 
by Yamabico to explore its world space. A simple example of the real automated cartogra- 
phy is illustrated starting with Figure 8.1 (a). The world W is a six-sided, L-shaped bound- 
ary polygon with no hole polygons. Yamabico is first placed in an arbitrary configuration 
such that no sensor is less than minimum range ¢.om any edge of W. In Figure 8.1 (a), Yam- 
abico rotates counterclockwise to align itself with the closest edge extracted from the rota- 
tional scan. Then Yamabico moves translationally until the forward sensor encounters an 
obstacle forward as illustrated in Figure 8.1 (b). The translational scan ends o from the ob- 
structing boundary. Yamabico now constructs its first partial world from sonar data. PW, = 
(Po, ©), with P, = (Ej, e;, next) where E; = {e7, €2, €3, €4, €5, €6). Edges e2 and eg are 
“real” and €7, €3, e4, and és are “inferred”. Edges e; and e3 are constructed perpendicular 
to Yamabico translational sc.n at a distance o from the endpoints of the translational scan. 


Edge e, is an “inferred” edge constructed to bound the region beyond sensor range. Edge 
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(a) Rotation to Identify Segments 


PW, = (Po, @) 
Po = (E}, ej next) 


E1 = (27, €2, €3, €4, &5, 6} 


world edges 
inferred line segment 


teal line segments 





(b) First Translational Scan 
Figure 8.1 Example of Yamabico Automated Cartography 
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PW, = (Po, ©) 
Po = (E2, €;, next) 


Ey = (€}, €2, €3, €4: €5, &G, €7} 


world edges 
inferred line segment 


real line segments 





(a) Second and Third Translational Scan 


PW; = (Po, @) 
P, = (E3, €;, next) 
E3 = {e1, €2, €3, C4, €5, €6, €7, Eg} 


world edges 


inferred line segment 


real line segments 





(b) Scans to Explore Inferred Edges 
Figure 8.2 - Example of Automated Cartography 
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4 is constructed two meters to the right of the translational scan. Edge es is an “inferred” 
edge constructed to connect the discontinuity between e, and ég. 

In Figure 8.2 (a), Yamabico turns left and scans “inferred” edge e; and then turns 
left and investigates “inferred” edge e,. The partial world is now PW>2 = (Py, ©), with P, 
= (E>, e;, next) where E> = {e7, e2, €3, 4, €5, 6, €7}. Edges e2, €3, and e7 are “real” and 
edges e), €4, es, and ég are “inferred”. 

In Figure 8.2 (b), Yamabico turns left and performs a translational scan on edge 
e;, then the next_scan_config function p'=ns a path to investigate edge és in Figure 8.1 (b). 
The resulting partial world is PW; = (Po, ©), with P, = (E3, e;, next) where E3 = {e), €2, 
3, €4, €5, C6, €7, €g, €g}. Only four “inferred” edges remain; e4, és, é¢, and e7 in Figure 8.1 
(b), the rest of the edges are “real”. 

In Figure 8.3 (a), Yamabico turns left and scans “inferred” edge e7 of PW3. The 
new partial world is PW, = (Po, ©), with P, = (Eq, e7, next) where Ez = {e7, €2, €3, €4, 5, 
£61 €7, €g, €g, €79, €71}. The remaining “inferred” edges are e4, €5, €g, €7, and ég. 

In Figure 8.3 (b), Yamabico turns left again and scans parallel to “inferred” edge 
€g. This scan moves Yamabico through the unexplored region bounded by edges és, eg, and 
e7 in PW4. The resulting partial world PW, = W since there are no remaining “inferred” 


edges. PW, =(P,, ©), with P, = (Eg, e7, next) where Eg = {€], €2, €3, €4, €5, €6}. 


3. Algorithm 
The global spatial learning algorithm is designed to allow Yamabico to intelli- 
gently move about in a given world space and build a model of its environment. The algo- 
rithm is shown in Figure 8.4. This is a top down, global overview of the algorithm. The pur- 
pose of each subroutine is explained in the remaining parts of this section. 
The initialize function initializes Yamabico’s sensor system for cartography. The 
appropriate sensors are also enabled. Linear fitting and data logging for the enabled sensors 


are enabled for the side scanning sonars. Since the nominal robot speed of 30.0 centimeters 
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PW, = (Po, @) 
Ps = (Eg, €;, next) 
Bg = fey, €2, €3, lg, C5, €gs C7; Og, Cg, €j0, Cj p 
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(a) Left Turn to Examine an Inferred Edge 


PW, = (Po, @) 
P, = (Eg, ep next) 
Eg = (21, €2, €3, &gy 5, €) 
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(b) Last Translational Scan 
Figure 8.3 - Example of Yamabico Automated Cartography 











per second is too high for cartography, the proper robot speed is set. Files for storing sonar 
data and the robot’s location trace data are set up and initialized. 

The find_orthogonal_orientation function orients the robot with respect to the 
nearest flat surface in the world space. Since Yamabico starts with no prior knowledge of 
the world, an initial rotational scan is required. This rotational scan extracts visible portions 
of the detectable edges of W and provides sufficient information to determine the best path 
element for the first translational scan. The same rules that were used for the S; version of 
the algorithm are used in this algorithm. Figure 8.1 (a) shows a robot placed in an arbitrary 
configuration in a simple closed workspace. The find_orthogonal_orientation function 
commands Yamabico to slowly rotate 360°. Sonar scans the world during the rotation. The 
extracted line segments are examined to determine the proper initial orientation of the robot 
and to chose an appropriate path element for the first translational scan. All of the extracted 


line segments are evaluated and the orientation of the closest line segment of sufficient 


spatial_learn() 
{ 


CONFIGURATION C = (0, 0, 0, 0); 
Partial World *PW = (©, ©); 
Path_List *PL = ©; 


initialize(PW); 

find_orthogonal_orientation(&C); 

while(not complete(PW)) 

{ 
translational_scanning(&PL, &C, &PW); 
PL = next_scan_config(&PW, &C); 

} l* end while */ 

return PW; 

} * end spatial_learn() */ 





Figure 8.4 Yamabico Cartography Algorithm 








length is adopted as the robot’s initial orientation. This function returns a path element suit- 
able for the first translational scan. 

Next the algorithm enters a “while loop”. The complete function evaluates all 
edges of the current PW. This function checks the PW for spatial consistency and for the 
absence of “inferred” edges. A NULL partial world or one with any “inferred” edge greater 
than © in length is considered incomplete. If the boundary polygon P,, consists of all “real” 
edges and the hole list is NULL, then the world is evaluated as complete. If the P, has all 
“real” edges and all of the hole polygons on the hole list H; ¢ H have all “real” edges, then 


the PW is considered complete. The function returns TRUE if the PW is complete or returns 
FALSE if more scanning is required. 

The translational_scan function moves Yamabico on a straight line path element 
parallel to the edge identified by the find_orthogonal_orientation function or the 
next_scan_config function. Yamabico continues the translational scan until one of the two 
forward looking sensors detects an obstacle. Following the translational scan, all sonar ex- 
tracted line segments from the side looking sonars are merged into the partial world model 
(PW). The PW constitutes a free space model of the area Yamabico has just scanned during 
a single straight line translation scan. Figure 8.1 (b) shows a PW derived from sonar data 
from Yamabico’s scan from point A to point B. 


During the automated cartography, dead reckoning errors accrue due to Yamabi- 


co’s motion. The automated cartography algorithm corrects this error in two ways; (1) wall 


following dead reckoning error correction and, (2) automated landmark selection and cor- 
rection during DFS backtracking. 

The wall following dead reckoning error correction operates under the assump- 
tion that the world space contains some long straight edges. When wall following is started, 
a path element parallel to the wall and at a distance less than B from the wall is calculated. 
Yamabico follows this path element and corrects some dead reckoning error using linear 


fitting sonar data extracted by scanning the wall. Yamabico’s distance to the wall and its 





orientation with respect to the wall are periodically corrected using the odometry correction 
algebra presented in Chapter V. 

During world space exploration Yamabico records landmarks suitable for odom- 
etry correction. A good landmark is characterized by a detectable discontinuity in a world 
space edge. During the DFS backtracking process, these landmarks may be used for full 
odometry correction. Unfortunately, an automatically recognized landmark has a configu- 
ration error tied to Yamabico’s dead reckoning error at the time it is recorded. Therefore, a 
dead reckoning error correction using an automatically recorded landmark reduce dead 
reckoning error to a value close to the value at the point where the landmark was originally 
recorded. 

The next_scan_config() function evaluates the geometry of the current partial 
world to determine the best path for the robot to follow to continue the world space explo- 
ration. The next_scan_config() function runs at the end of each translational scan. The func- 
tion examines all edges in the current partial world and determines which edge is the best 
“inferred” edge to investigate next. The function performs a simple path planning function 
to plan a path from the robot’s current configuration to the next translational scan. This path 
is planned using a depth first search strategy in which Yamabico stores it path using the 
Path_List PL as the exploration progresses and backtracks back along PL to the next in- 
ferred edge. This behavior is illustrated in Figure 8.5. This backtracking behavior allows 
the robot to explore its world using a depth-first-search strategy which is equivalent to an 
“in order” traversal of the free space in the world [Manber 89]. 

In Figure 8.5 Yamabico starts out at point 1 and moves to point 2 for a translation- 
al scan. During this scan, three “inferred” edges are identified; e,, e,, and e,. Since e, is the 


closest “inferred” edge to point 2, Yamabico backtracks back along its first translational 


scan to point 3 and then turns right to investigate edge e,. Yamabico scans from point 3 to 


point 4. At point 4 the next closest “inferred” edge is e,. At this point Yamabico turns 


around again an scans from point 4 to point 5. At point 5 an open area exists to Yamabico’s 











Figure 8.5 - The next_scan_config() Depth First Search Exploration Behavior 


left, so it turns left and scans to point 6. At point 6, open area again exists to the left so Yam- 
abico turns left and then scans from point 6 to point 7. 

The next_scan_config function algorithm is given in Figure 8.6. This function 
guides Yamabico to scan “inferred” edges using a depth first search strategy. This function 
favors paths that increase the overall mapped area as quickly as possible. If the PW is eval- 
uated as incomplete, the robot uses the next_scan_config() function to determine where to 
next move for translational scanning. This feature underscores the fact that the robot must 
move to a new scanning configuration (C) based upon the derived partial world model 
(PW). The next scan configuration is selected to optimize the open area mapped by Yam- 


abico. The following rules apply: 
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next_scan_config(PW, PL, C) 
Partial World *PW; 
Configuration *PL; 
Configuration C; 


{ 
EDGE *closest_edge; 
CONFIGURATION *path_element; 


if (obstacle forward and clear left) { 
path_element = turn_left(); 
add_path_to_list(path_element, PL); 
} 

else { 

closest_edge = find_closest_edge(PW, PL, C); 

DFS_backtrack(PW, PL, C); 

move(PL); 


} 
} * end next_scan_config() */ 


Figure 8.6 - The next_scan_config Algorithm 


(1) If an obstruction is forward and the left side sensor reports no obstructions to 
the left, then Yamabico tus left 90° for the next translational scan. The path element for 
the scan is added to PL. 

(2) Otherwise Yamabico turns 180° and backtracks along DFS to the closest in- 


ferred edge suitable for investigation. 


C. SUMMARY 

An algorithm for Yamabico’s automated robot cartography using ultrasonic range find- 
ers is developed in this chapter. The algorithm acts in a greedy fashion to continuously in- 
crease the planar area mapped by the vehicle. Intelligent vehicle motion is required to reach 


all portions of the world space. Robot motion to explore the world space results in odometry 





error. The algorithm uses heuristic and spatial reasoning to correct some odometry error in 
real time as the cartography is performed. 

The real sensor limitations of finite range, limited incidence angle for returns, and 
specular reflection tend to complicate the cartography problem. The algorithm presented in 
this chapter provides a means to work within these physical limitations. The real cartogra- 
phy algorithm is implemented on the autonomous mobile robot Yamabico-11 and the ex- 


perimental results are explained in Chapter IX. 


161 





IX. EXPERIMENTAL RESULTS AND CONCLUSIONS 


Experimental results from Yamabico motion control, dead reckoning error correction, 
and cartography experiments are presented in this chapter. Some experimental results from 
the development of the motion control portion of the MML language are presented. Several 
representative experiments are included, however, hundreds of additional successful and 
not so successful experiments were conducted for this dissertation. Space considerations 
prohibit including all of the experiments. A “user.c” robot command file and a plot of the 
vehicle’s observed trajectory are included with each experiment. 

The dead reckoning error correction experimental results are presented in the form of 
two representative examples. One is a single landmark experiment with Yamabico moving 
in a nine meter long racetrack pattern and the second is a more complex three landmark 
experiment with approximately 30 meters of travel per revolution. 

The automated cartography experiments demonstrate Yamabico’s ability to map a 
closed world space using only ultrasonic range finders. Yamabico path tracking capability 
and periodic odometry correction using landmarks are required to derive a high quality map 
from an indoor world. The conclusions drawn from the experimental results are then 


summarized. 
A. MOTION CONTROL EXAMPLES 


1. Observation Plan 
Early experiments were designed to test Yamabico’s ability to follow straight and 
curved paths. These tests revealed that an extensive redesign of most of the existing MML 
kernel was required. The general plan was to evolve the previous configuration-to-config- 
uration tracking system into the new path tracking system [Kanayama 91a]. The first goal 
was to program Yamabico to track along a straight line or a circular arc path elements. Ob- 


servations and measurements of Yamabico’s motion were planned using marks made on 
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the floor of the test area. Yamabico’s expected and actual position were compared. Path 
tracking was thought to provide smoother vehicle motion and improved odometry correc- 
tion capability as a result of changing the method of tracking. 

The observation plan is a series of representative tests of the robot’s ability to 
track the path elements described in Chapter IV. The “user.c” specifications for several 
simple test programs are given along with plots of the robot’s motion. These test programs 
are samples from the test battery designed to test Yamabico’s locomotion functions. Many 
combinations of locomotion functions were not tested, because exhaustive testing is intrac- 
table due to the large number of possible combinations of vehicle motion commands. Ad- 
equate testing was conducted to eliminate bugs in the most used parts of the motion control 
software and to provide a reasonable user confidence level. Troubleshooting and improve- 


ment of the motion control software continues in the Yamabico research group. 


2. Observation Results 

The first locomotion test for MML was simple line tracking. The “user.c” file and 
Yamabico’s trajectory plot is shown in Figure 9.1 Yamabico is given its starting configu- 
ration in the user’s global coordinate system using the set_rob command. This starting con- 
figuration is called start in this program and any valid ‘C’ variable is an acceptable config- 
uration variable in the MML language. The def_configuration function is used to define 
configurations needed in the course of the “user.c” program. In this program, the start con- 
figuration is defined by the parameters x = 0.0, y = 100.0, @ = 0.0, and k = 0.0. The set_rob 
function is required at the beginning of every “user.c” file to initialize Yamabico’s odom- 
etry configuration. Yamabico is then commanded to track the first path element using the 
bline( &first) function. This function commands Yamabico to track the straight path ele- 
ment starting at its current configuration and passing through the configuration x = 100.0, 
y = 100.0, and 6 = 0.0. Since x = 0.0 for both first and second and « = 0.0 means a zero 
curvature path element, therefore these path elements are straight line path elements. 


Smooth motion from the first configuration onto the second path element is observed as 
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#include “mml.h” 


user() 

{ 
CONFIGURATION start; 
CONFIGURATION first; 
def_configuration(0.0, 100.0, 0.0, 0.0, &start); 
def_configuration(100.0, 100.0, 0.0, 0.0, &first); 
def_configuration(0.0, 0.0, 0.0, 0.0, &second); 


set_rob(&start); 
bline(&first); 
line(&second); 


YAMABICO! TRAJECTORY DATA — 


Y fev, 


200 
x-axis (cm) 


Mon Jun 21 17:39:04 1993 y-axis (cm) 





Figure 9.1 Simple Line Tracking 
shown in Figure 9.1. The vehicle was observed to closely track the commanded line seg- 
ments based upon hand measured marking on the floor. In no case did the vehicle’s position 


deviate from the expected path by more than 2.0 centimeters. In this experiment, vehicle 
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guidance commands were issued at a 100 Hz rate, Yamabico’s velocity is 30.0 centimeters 
per second, and the size constant is 20.0. The speed and size constant default to nominal 
values since they are not specified by the user. Program illustrates simple lane changing be- 
havior that could be used for obstacle avoidance. 

The second experiment demonstrates circular path element tracking. A circle is 
specified in the same manner as a straight line using the line function, except the curvature 
of the circular path element is not zero (x #0). The “user.c” file and the plot of the robot’s 
trajectory is shown in Figure 9.2. Notice that only six lines of MML code are used to specify 
this complex robot behavior. Yamabico is given the initial configuration start such that x = 
0, y = 0 and the vehicle initial orientation is 8 = HPI which means 90° with respect to the 
x-axis. Yamabico is commanded to track a circular path first with the configuration x = 200, 
y =-100, 8 =0, and k =-0.1. The x and y parameters specify the starting point for the circle, 
6 gives the orientation, and « = -0.1 means the circle’s radius r = 1/K = 1/-0.1 = -100.0 cen- 
timeters. The negative value of the radius indicates a clockwise direction for the first path 
element. Further MML implementation details appear in Appendix A. Yamabico leaves its 
starting configuration and turns sharply to the right. The sharpness of this turn is deter- 
mined by the value of the size constant (s,). Once again the nominal speed and size constant 
values are used by default. Once started Yamabico immediately begins tracking the circular 
path element first. It continues tracking the circle indefinitely since no stopping command 
is issued. At no point during this experiment did Yamabico’s trajectory deviate by more 
than 1.0 centimeter from the expected path element trajectory. 

The third example demonstrates circular backward line tracking. Yamabico tracks 
the smaller inner circle as illustrated in Figure 9.3. The end of the backward line is located 
at the configuration x = 100.0, y = 0.0, and 8 = 0. When Yamabico reaches the end of the 
backward line it then switches to tracking the outer circle (second). If first was the last path 
in the “user.c” file Yamabico would have stopped at the end of the backward line. The sec- 
ond path element is a counterclockwise circular path element with radius r = 1/«K = 1/-0.1 


= +200.0 centimeters. In this experiment robot velocity is set to 15.0 centimeters per second 


165 











#include “mml.h” 


user() 
{ 
CONFIGURATION start; 
CONFIGURATION first; 
def_configuration(0.0, 0.0, HPI, 0.0, &start); 
def_configuration(200.0, -100.0, 0.0, -0.01, &first); 


set_rob(&start); 
line(&first); 


07: 
y- 
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Figure 9.2 Circle Tracking 
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#include “mml!.h” 








user() 

{ 
CONFIGURATION start; 
CONFIGURATION first; 
CONFIGURATION second; 


300 


100 


-100 





def_configuration(0.0, 100.0, -HPI, 0.0, &start); 
def_configuration(100.0, 0.0, 0.0, 0.0, 0.01, &first); 
def_configuration(100.0, -100.0, 0.0, 0.005, &second); 


size_const(20.0); 
speed(15.0); 
set_rob(&start); 
bline(&first); 
line(&second); 





-300 -200 -100 0 100 200 300 400 500 


x-axis (cm) 


Figure 9.3 Circular Backward Line Tracking 











and the size constant is 20.0. Yamabico’s deviation from both of the intended path elements 
was less than 2.0 centimeters. 

In Figure 9.4, Yamabico is commanded to track a path consisting of four sequen- 
tial path elements. The first path is a straight line path given by the configuration start. The 
second path element is a parabola given by the first specification. The parabola’s focus is 
at x = 300.0, y = 100.0, and the directrix is given by the configuration x = 0.0, y = 0.0, 8 = 
0.0. The parabolic path element first has five component parameters as discussed in Chap- 
ter IV and Appendix A. The path elements start and first intersect at the point x = 200.0, 
and y = 100.0. Yamabico calculates a leaving point on the start path element to allow for a 
smooth transition to the first parabolic path element. Then Yamabico tracks the parabola 
first. The first and start path elements intersect and Yamabico calculates the next leaving 
point on the parabolic path element first. When this leaving point is reached Yamabico 
switches to tracking the straight line path element start. The path elements start and third 
also intersect. Yamabico then calculates the next leaving point on the start path element. 
When the last leaving point is reached, Yamabico switches to tracking the third path ele- 
ment. The entire program represents 30.0 seconds of robot motion at the default nominal 


velocity of 30.0 centimeters per second. The distance constant (s,) controls the path ele- 


ment transition sharpness, the default value is 20.0 centimeters. 

The last example is a demonstration of cubic spiral tracking. In Figure 9.5 Yam- 
abico is started at the origin and commanded to track a cubic spiral from the start configu- 
ration to the first configuration. Two cubic spirals are actually tracked, one before the point 
of inflection and another after it. At no point during this experiment did Yamabico’s trajec- 
tory deviate by more than 2.0 centimeter from the expected path element trajectory. Cubic 
spiral path tracking is described in greater detail in another publication [Fish 93]. 

Yamabico closely tracked the intended paths in all five experiments. The motion 
control results indicate that the path tracking technique yields extremely small motion con- 
trol errors for vehicle travel over a given distance. Odometry error is experimentally mea- 


sured in section B of this chapter. 
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#include “mm!.h” 


userQ) 


{ 


CONFIGURATION start; 
PARA first; 
CONFIGURATION third; 


def_configuration(0.0, 200.0, 0.0, 0.0, &start); 
def_parabola(300.0, 100.0, 0.0, 0.0, 0.0, &first); 
def_configuration(700.0, 200.0, -HPI, 0.0, &third); 


set_rob(&start); 
line(&start); 
parabola( &first); 
line(&start); 
line( &third); 


YAMABICO: TRAJECTORY § 


400 
x-axis (cm) 


Figure 9.4 Parabolic Tracking 
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#include “mml.h” 


user() 

{ 
CONFIGURATION start; 
CONFIGURATION first; 


def_configuration(0.0, 0.0, 0.0, 0.0, &start); 
def_configuration(200.0, 0.0, HPI, 0.0, &first); 


size_const(20.0); 
speed(15.0); 
set_rob(&start); 
config(&first); 
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Figure 9.5 Cubic Spiral Tracking 
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B. ODOMETRY EXPERIMENTAL RESULTS 


1. Experimental Plan 

Two kinds of odometry correction experiments are performed using Yamabico- 
11. To verify the fundamental correctness of the algorithm, first a simple racetrack path 
with a single landmark is used. Yamabico moves repeatedly around this racetrack path 
which is composed of four separate path elements. Yamabico is programmed to make an 
odometry correction once per lap using a single landmark. The amount of odometry error 
is measured by hand and by examining logged sonar data. These measurements serve to es- 
tablish a correlation between type of motion and rate of accumulation of odometry error. 

The second experiment is slightly more complex. The multiple landmark experi- 
ment is conducted to prove the utility of this algorithm for more sophisticated vehicle nav- 
igation. The sparse assignment of landmarks serves as a worse case to prove the utility of 
the odometry correction and precise vehicle tracking algorithm in an indoor environment. 
The experimental plan is: (1) program the robot to move in a repeating pattern with about 
five left and right turns; (2) use several fixed landmarks that are scanned by the side looking 


sonars to perform odometry error corrections. 


2. Experimental Results 

The first MML program executes an oval path for Yamabico. This is a skeleton 
of the whole program which is much longer and contains other functions. The motion con- 
trol portion of the program and the resulting robot motion are shown in Figure 9.6. The dead 
reckoning correction code appears in Appendix D. In each lap of this oval path execution, 
the odometry error correction is performed and the error configuration € is recorded. The 
total distance traveled is 9.14 meters per lap. In this experiment, only one landmark is used 
for the odometry error detecting purpose. Table 9.1 shows the raw experimental data for 
Yamabico traveling ten laps at 25 cm/sec. Notice that the results show the error configura- 


tion for each lap is small and nearly equal. This proves that Yamabico’s motion control and 








#include “mml.h” 


user() 


{ 


CONFIGURATION start, first, second, third, fourth; 
int laps = 10; 

int lap_count = 0; 

def_configuration(1200.0, 65.0, 0.0, 0.0, &start); 
def_configuration(1100.0, 65.0, 0.0, 0.0, &first); 
def_configuration(1500.0, 65.0, 0.0, 0.02, &second); 
def_configuration(1700.0, 165.0, PI, 0.0, &third); 
def_configuration(1200.0, 165.0, PI, 0.02, &fourth); 


set_rob(&start); 

while (lap_count < laps) 

{ 
line(&first); 
line(&second); 
line(& third); 
line(& fourth); 
++lap_count; 
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Figure 9.6 Single Landmark Odometry Correction Experiment Code 
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Table 9.1 ODOMETRY ERROR CORRECTION (25 CM/SEC) 
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as desired. 

This experiment was repeated at various robot speeds. The average error config- 
uration over ten laps at speeds of 10, 15, 20, 25, and 30 cm/sec are shown in Table 9.2 be- 
low. As in the previous experiment, error correction was made each lap. Notice that the av- 
erage dead reckoning error per lap tends to increase with increasing robot velocity. In Fig- 

Table 9.2 AVERAGE ODOMETRY ERROR AS A FUNCTION SPEED 
(degrees) 
0.0020 | 0.11459 


0.0045 0.25783 


. 











ure 9.7 the odometry corrections for ten laps on the racetrack pattern are plotted. The largest 
odometry correction is the first correction since the robot is intentionally displaced from the 
proper starting configuration (start = (1200.0, 65.0, 0.0, 0.0)). The odometry error correc- 
tion code corrects this initial placement error as if it were a large odometry error. Subse- 
quent dead reckoning error corrections are smaller and consistent indicating Yamabico’s 
odometry parameters could be tuned to improve the results of this experiment. This exper- 
iment also shows that odometry error on the robot is small and repeatable. Odometry error 
could be reduced by self correction of odometry parameters in software, however this ex- 
periment was not conducted. 

Yamabico’s odometry error for ten laps at 25.0 centimeters per second is plotted 
in Figure 9.8. The location of the tail of each arrow is the x, y magnitude of the odometry 
error and the orientation of each arrow is the 8 component of the odometry error. Notice 
that most odometry errors fall within a small cluster of values. The general trend observed 


was that Yamabico tends to turn slightly more than x radians programmed. Overall the er- 
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Figure 9.7 Ten Robot Odometry Corrections at 15 centimeters per second 
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Figure 9.8 Robot Odometry Error at 25 cm/second 


ror is small, but the author believes that wheel slippage during the turns contributes to the 
larger odometry errors at the higher speeds. Mismatch between the left and right wheel 
drive motors also causes some error because the same pulse width modulation curve is used 
for both motors. 

Table 9.3 MULTIPLE LANDMARK ODOMETRY CORRECTION 
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The multiple landmark odometry experiment is conducted in an unmodified in- 







door environment. Three landmarks constitute the robot’s abstract geometric model of the 


world and are used for the purpose of odometry error detection and correction. Multiple 
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landmark correction behavior is closer to the desired goal of autonomous robot navigation. 
A section of the hallway on the fifth floor of a building at the Naval Postgraduate Schoo! is 
used. Average odometry corrections for the multiple landmark experiment are shown in Ta- 
ble 9.3. A row in the table stands for a correction result at a numbered landmark. Relatively 
small and consistent odometry corrections are also observed in this experiment. One lap of 


the multiple landmark correction experiment is shown in Figure 9.9. The landmarks are 


—gwooden door 
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Figure 9.9 Multiple Landmark Odometry Correction 
numbered to correspond with the landmark numbering in Table 9.3. This result shows that 


only three landmarks provide enough dead reckoning error correction for Yamabico to nav- 


igate a 30 meter circuit in an ordinary indoor environment. 
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C. AUTOMATED CARTOGRAPHY EXPERIMENTAL RESULTS 


1. Experimental Pian 

The following automated cartography experiments are planned to test the algo- 
rithm’s implementation on a small, orthogonal world using Yamabico-11; 

(a) Orthogonal world alignment - Place Yamabico in a variety of configurations 
in the test world. Run the find_orthogonal_orientation function alone at each configura- 
tion. Quantify the implementation’s precision with regard to orthogonal alignment. 

(b) Straight hallway following - Program Yamabico to follow a hallway with ob- 
stacles and doorways, and record its path. The goal is to have Yamabico travel down the 
centerline of the hallway while correcting dead reckoning errors. Line segments extracted 
from both the left and right hand walls of the hallway are used to align Yamabico to the 
centerline of the hallway. Line segments must have sufficient length (100 cm) and be lo- 
cated near their expected configuration to be acceptable for correcting dead reckoning er- 
rors. 

(c) Partial world building by a single translational scan - In this experiment 
Yamabico is commanded to move several meters on a path orthogonal to the world space. 
A partial world is built by Yamabico based upon the line segments extracted from this sonar 
scan. The partial world built is compared with hand measured maps for accuracy. 

(d) Full automated cartography- The full partial world building automated car- 
tography experimental plan involves perform cartography experiments on a small orthog- 
onal world with holes. The experimental area is an office building hallway, modified with 
artificial barriers to keep the size of the world reasonably small. Cardboard boxes serve as 
the obstacles holes in this world space. Yamabico is placed at an arbitrary configuration in- 
side of this world space and the cartography program is started. The cartography program 
runs to completion. The map built by Yamabico is then transferred back to the host com- 


puter for analysis. The polygonal world map derived from the robot’s world space explo- 
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ration is compared to the actual hand-measured map of the world space. Reasons for dis- 


crepancies are analyzed. 


2. Experimental Results 

The code for the robot automated cartography experiments is listed in Appendix 
E due to its length. 

(a) The find_orthogonal_orientation function aligns Yamabico’s internal coordi- 
nate system orthogonal to the world space based upon the line segments extracted from a 
360° rotational scan. This function chooses the closest extracted line segment longer than 
20 cm for alignment. Since four line of the segments are parallel, the initial scan may prop- 
erly take one of two directions. Yamabico is commanded to rotate the shortest angular dis- 
tance to align parallel with the closest extracted line segment. In Figure 9.10, Yamabico is 
placed in the hallway with no prior knowledge of the world space. Yamabico rotates clock- 
wise 2x radians and extracts five line segments from sonar. These line segments are inter- 
preted to allow the robot to determine the orientation of the first translational scan. In all 
experiments conducted Yamabico’s orientation was within £3° of parallel to the closest or- 
thogonal surface. Certain situations were observed to improve the accuracy of this function. 
For instance the close proximity of a long flat wall improves accuracy. 

(b) In Figure 9.11, a hallway following experiment is shown. In this experiment, 
Yamabico used a straight hallway assumption to provide odometry correction as it moved 
approximately 20 meters down the hallway. This same hallway following experiment has 
been used to map a hallway 70 meters long. The hallway is 246.0 cm wide. Yamabico start- 
ed at the configuration x = 0.0, y = 123.0, 8 = 0.0 and tracked a straight line path element 
down the center of the hallway. Side mounted sunars were used to scan the right and left 
hallway walls. The sonar data from the side scanning sonars is used for periodic odometry 
corrections based upon the assumption that the hallway walls on either side of the robot are 
straight. The sonar data is extracted and processed as line segment data. Line segments (or 


edges) of sufficient length provide distance and orientation information for Yamabico to 
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Figure 9.10 The Find Orthogonal Orientation Experiment 


179 








(wo) stTxe-x 


0002 OOST 0O00T 00S 0 


009- 


OPP OSPPSTTITOTTTTereereeTTeTreerrererrereriririrriterrerrieiricetiireer ere rere ee eer ite Settee eee ee ee eee eee eee eee eres See eee renner er rer Terre re reer err errr ener re ney ae 





try reset 






* 
: 
: 
: 
: 
: 
: 
: 
. 
4 
. 
> 
‘ 
‘ 
Peers Seer rer er ere reer e eee eee eee eee eee ress Sry 


me 





co 
Odo 





yea Avayey 4] 
peccoe Deb espns ahadosee Senses, ; ame ure nnearah. 


: : 
povesecsorees Ve Bev rower w ecw ne nfo mmm mem mem meters wow ares onset ha ee ve men a 





elie Sih ta en OAS oe a a OOP 





sees WLW > UWNO$ 
ae WLW L Y¥NO$ 





Figure 9.11 Hallway Following Cartography Experiment 
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fix its position relative to the center line of the hallway. Yamabico then performs an odom- 
etry estimate reset to correct its configuration with respect to the center line of the hallway. 
Since Yamabico has no prior knowledge of the hallway, the translational error (absolute 
distance moved down the hallway) in the odometry estimate cannot be corrected. 

The sonar data extracted from the side-scanning sonars is also stored as cartogra- 
phy data. Hallway following with odometry correction is an important component of auto- 
mated indoor cartography since many office buildings have hallways narrow enough so 
Yamabico can travel down the center of the hallway and sense both walls. This program is 
robust since Yamabico stays in the center of the hallway despite some sensor noise. Noisy 
segment data arises from specular reflections, sensor noise, and people walking in the hall- 
way. This noise is filtered out in part by (1) rejecting non-orthogonal sonar segments, (2) 
rejection of short segments, and (3) rejection of segments beyond the sonar’s effective iin- 
ear fitting range. Yamabico configuration did not deviate from the reference path element 
by more than 40.0 centimeters during these experiments. 

(c) In Figure 9.12 a partial world constructed from a single 14 meter translational 
scan down a hallway is shown. All “real” edges are constructed from extracted sonar seg- 
ment data. The “inferred” edges are constructed at the beginning and at the end of the trans- 
lational scan to bound unexplored area. The elevator foyer area on the right hand side of the 
translational scan appears as 60.0 centimeter deep break in the right hand wall. An “in- 
ferred” edge parallel to Yamabico’s translational scan path and two meters from the robot’s 
path is constructed to indicate a portion of the world is out of sonar range. The elevator foy- 
er is about five meters deep. The partial worlds constructed in this experiment did not de- 
viate from hand measured map by more than 10.0 centimeters. 

(d) The full partial world experiment derived more complete maps of the world 
space. In Figure 9.13, Yamabico builds a more complex map of the hallway portion of an 
office building. Yamabico is placed at position 1 with an arbitrary orientation in Figure 
9.13. Yamabico rotates 360° and extracts several line segments representing portions of the 


hallway within sonar range. Yamabico aligns itself with the parallel edges extracted from 
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the rotational scan and then performs a translational scan from position 1 to position 2. 
Yamabico stops at position 2 because an obstacle is detected forward. This first translation 
scan is about 14 meters long and a partial world similar to the one shown in Figure 9.12 is 
built in Yamabico’s memory. 

Yamabico then turns around to the right and moves from position 2 to position 3 
to investigate the “inferred” edge bounding the opening to the elevator foyer. The next 
translation scan move Yamabico from position 3 to position 4. The doorways for the two 
elevators on Yamabico’s right hand side are extracted as edges. Yamabico stops at position 
4 because an obstacle is detected forward and then turns left 90°. 

Next Yamabico performs a translational scan from position 4 to position 5. Then 
takes another left turns to scan to position 6. The entire elevator foyer region has been 
scanned. Next automated cartography algorithm seeks the next “inferred” edge for explo- 
ration. This edge is near the starting position (position 1). So Yamabico moves from posi- 
tion 6 down the centerline of the hallway to position 7. It stops at position 7 due to the pres- 
ence of a barrier. The derived partial world matches a hand measured map of the same 
world space within 25 cm for all extracted line segments. With additional experiments and 
program tuning this error value could be further reduced. 


D. SUMMARY 
Reliable path tracking control using path elements has been experimentally tested. In 


no case did Yamabico deviate significantly from the expected path. The maximum ob- 
served deviation was less than two centimeters. Vehicle dead reckoning error correction 
has been experimentally verified. These experiments used Yamabico’s side scanning so- 
nars to detect static landmarks in the world space. An algebraic comparison between the 
expected and actual landmark configuration was used to determine dead reckoning error. 
Errors were small and consistent. Yamabico’s configuration after a each odometry correc- 
tion was within one centimeter of the correct configuration which indicates dead reckoning 


error correction works well with a precision limited by sensor resolution. 
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Figure 9.12 Single Translational Scan Experiment 
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Figure 9.13 Robot Cartography Experiment 
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The automated cartography experiments demonstrated simultaneous dead reckoning 
error correction and world space mapping. The find_orthogonal_orientation function reli- 
ably aligns Yamabico’s coordinate system orthogonal to the world space. Maps built by 
Yamabico using the automated cartography algorithm closely match hand measured maps. 





X. CONCLUSIONS 


In this dissertation, a complete automated cartography system for an autonomous 
mobile robot is defined, instantiated, and evaluated. The component parts of this system are 
path tracking control, dead reckoning error correction, world space exploration, and map 
representation using a polygonal world model. MML provides the mid-level vehicle 
control commands necessary for smooth dead reckoning error correction and obstacle 
avoidance using geometric path elements. MML also provides the necessary sonar, 
odometry correction and input/output functions for mobile robot experiments. The vehicle 
odometry correction provides the periodic dead reckoning error correction needed for 
precise cartography. The automated cartography algorithm controls robot motion so all 
accessible areas of the world space are examined by the robot’s sensors. The algorithm 


provides the constructs for proper map representation as the world model is built. 


A. SUMMARY OF CONCLUSIONS 

A series of algorithms for ideal automated cartography are developed. These algo- 
rithms are used to study the structures for map representation for cartography as well as the 
necessary robot motion rules for world space exploration. The correctness of each algo- 
rithm is proved. The complexity of these algorithms was also examined to ensure cartogra- 
phy could be accomplished in a reasonable amount of time. 

Successful robot cartography experiments using a real autonomous mobile robot are 
conducted for this dissertation. Abstract 2D maps of a world space are built by Yamabico- 
11. The features of this map closely match maps built from hand measurements. The auto- 
mated cartography experiments prove that ultrasonic sonars can be used exclusively to 
build useful maps of an indoor orthogonal environment. Despite data scatter and odometry 


error, Yamabico builds a fairly good map of its environment from sonar data. 
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A 2D transformation algebra for analyzing mobile robot motion is developed in this 
dissertation. This algebra is used to develop an abstract, general-purpose means of mobile 
robot localization and odometry correction. This method is a simple application of group 
theory that requires very little computational overhead. Dead reckoning error correction in 
real-time is experimentally verified using Yamabico-11. These experiments show that 
odometry error can be held to a small, relatively constant value despite repeated vehicle 
motion. In the worst case, average odometry error is 2.54 centimeters in distance and 1.04 
degrees in orientation over a 914 centimeter course. The multiple landmark experiment 
proves that the odometry correction algorithm works well even with sparsely placed land- 
marks. The algorithm is robust, as shown by continued odometry correction despite land- 
marks occasionally being missed. Periodic, automatic odometry correction allows for sus- 
tained autonomous robot operation that is not limited by dead reckoning error buildup. This 
automatic, low-overhead dead reckoning error correction task is implemented in the MML 
software system as one of the available functions that can be activated by a function call. 
This capability is extremely useful when a sustained robot motion is required. Dead reck- 
oning error Correction using abstract algebra supports automated cartography. 

The MML motion subsystem is an effective and abstract method to specify robot mo- 
tion. Short, simple command listings provide surprising complex robot motion. Experimen- 
tal results from path tracking tests on Yamabico-11 demonstrated precise mobile robot con- 
trol. The smooth robot motion resulting from the path tracking technique reduces wheel 
slip, thus improving the overall robot odometry. The path element concept allows higher 
level command modules to specify robot motion using a short list of path element com- 


mands. Path elements allow a user or an artificial intelligence application to command the 


robot to execute a wide range of motion. Path tracking locomotion control using MML sup- 


ports automated cartography. 








B. CONTRIBUTIONS TO MOBILE ROBOTICS 

This dissertation describes software and hardware developments that enable an auton- 
omous mobile robot to perform automated cartography using ultrasonic range finders. This 
capability is limited to orthogonal, indoor regions. There are several important components 
required to perform this task. The capability to move in a purposeful manner based upon 
sensor input is required. Leonard used a preprogrammed “seed-spreader” pattern to control 
robot motion for gathering sensor data [Leonard 92]. This means some a priori knowledge 
of the shape of the world space was usea <0 Command the robot to move about in the ap- 
propriate pattern. This author believes that true automated cartography requires the robot’s 
exploratory path to vary with the size and shape of the area being mapped. Tight coupling 
of the sensor input and the vehicle motion is therefore required. The algorithm used for au- 
tomated cartography in this dissertation performs automated world space exploration. Us- 
ing this algorithm, the robot scans all accessible portions of the world with its sensors in 
order to perform the cartography. Since sensor input and robot motion are tightly coupled, 
no prior world space knowledge is needed to perform cartography. 

Robot motion control using a new path tracking algorithm is developed for controlling 
robot motion during the world space exploration phase. This control algorithm is described 
in detail in Appendix A. This technique allows Yamabico to perform odometry corrections 
in a very smooth and natural fashion. Odometry corrections are performed with respect to 
a geometric path instead of a single robot configuration. Periodic odometry estimate cor- 
rections using landmarks on the robot’s map allow for sustained robot operations with 
small, constant positional error. 

As a robot moves about exploring its assigned space, dead reckoning errors build up. 
The ability to perform localization using naturally occurring landmarks in a typical indoor 
area is another important contribution of this dissertation. While other authors have dem- 
onstrated localization [Leonard 92][Nehmezow 92]}[Cox 89][Crowley 85a], this disserta- 
tion extends this capability to automated landmark recognition. In other words, not only 


does the robot have the ability to self-correct dead-reckoning errors using external land- 
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marks, it also recognizes and catalogs landmarks appropriate for subsequent odometry cor- 
rection. Vehicle configuration calculations using the abstract algebra described in Chapter 
V greatly simplify robot odometry error correction coding. 

Since robot exploration of a world space requires robot motion and since this motion 
induces dead reckoning error, mapping and error correction must be accomplished togeth- 
er. The challenge of truly autonomous navigation lies in the fact that map building and 
odometry correction must be undertaken simultaneously [Leonard 92]. This dissertation 
develops both of these capabilities in parallel in order to allow the robot to build precise 
maps. All previous work has focused on either one or the other of these two tasks. This au- 
thor knows of no instance where map building competence and odometry correction have 
been performed simultaneously. Their interdependent nature bears out the fundamental 


need to develop these capabilities concurrently. 


C. SUGGESTIONS FOR FUTURE RESEARCH 

This dissertation research discussed the development of one system to perform auto- 
mated cartography in an indoor setting. Limiting this system to one sensor type placed se- 
vere, unrealistic restrictions on the robot’s cartography ability. Additional sensors added to 
Yamabico-11 such as vision, tactile sensors, and infrared range finders are needed to handle 
cartography in a more cluttered indoor environment. Sensor sweeps of a 2D plane during 
translational scanning could possibly extent robot cartography to build 3D maps of indoor 
spaces. 

Improved robot mobility such as the ability to open doors and climb stairs would great- 
ly improve robot cartography capability. Research on a special robot morphology is needed 
to map ventilation ducts and crawl spaces in buildings. In naval applications, cartography 
can be extended to inspections of void spaces and tanks on naval ships. These spaces peri- 
odically require hazardous human inspections for corrosion and loose fittings. 

A voice interface is currently under development to provide a more intuitive robot/hu- 


man. This system will enable Yamabico to receive voice commands. Additionally, voice re- 
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sponses by the robot would make it a more useful platform. Voice commands such as “go 
remap region B” could make Yamabico a good assistant to a human making a floor plan of 
a building. 

The improved utilization of ultrasonic sound is an important extension that hold a great 
potential. The existing sonars on Yamabico could be better utilized if the diagonal sonar el- 
ements were employed to examine hard-to-reach areas such as concave corners. Vastly su- 
perior sonar utilization in the animal kingdom tells us that much of the resolution and ca- 
pability of 40 KHZ sound waves in air is significantly under-utilitized [Nachtigall 86]. The 
investigation of frequency modulated sonar sensors in air is a useful extension to this dis- 
sertation. A software variable range gate and pulse length could significantly enhance the 
capability of ultrasonic sonar as a sensor. A shorter range gate is more appropriate in a 
close, cluttered part of a building whereas a long range gate and a longer pulse interval work 
better in open spaces. An automatic, adaptive sonar range gate and pulse width could sig- 
nificantly enhance cartography. 

A more powerful on board computer is desired to allow Yamabico-11 to perform faster 
processing. The video image processing is desired for navigation and object recognition in 
a cluttered indoor area. This is a computationally demanding task. A SPARC4 single-board 
supercomputer is currently being installed to support faster computations [Ironics 91]. A 
significant portion of the robot’s hardware device drivers are being rewritten to support the 
new central processing unit. 

A genetic algorithm approach to world space exploration using the Yamabico simula- 
tor to evolve a good exploration strategy in different world spaces might be worthwhile. 
The evolved cartography algorithm would need to be validated on the robot. This would 
require an extremely accurate simulator sonar model to predict sonar returns, the simple ray 
tracing model used in the current simulator is insufficient for this task. Lastly, specular re- 
flection would have to be modeled more completely. 

This dissertation has addressed the problem of automated cartography in autonomous 


mobile robots. The experimental results presented here are encouraging and show that the 
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automated cartography is suitable for the intelligent robot exploration of an indoor world 
space. Future robots will undoubtedly draw 3D models of existing buildings by exploring 
their interiors with sophisticated sensors. This dissertation is one step on the road to this 
goal. 
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INTRODUCTION 


Yamabico-11 is an autonomous mobile robot powered by two 12-volt batteries and is driven 
on two wheels by DC motors. These motors drive and steer the wheels while four shock absorbing 
caster wheels balance the robot. 

The master processor is a MC68020 32-bit microprocessor accompanied by a MC68881 float- 
ing point coprocessor. (This is the exact same CPU as a Sun-3 workstation). This processor has one 
Mbyte of main memory and muns with a clock speed of 16MHz. 

All programs on the robot are developed using a Sun 3/60 workstation and UNIX operating 
system. These programs are first compiled and then downloaded to the robot via a RS-232 link at 
a baud rate of 9600. The software system consists of a kernel and a user program. The kernel is 
currently 65,000 bytes and only needs to be downloaded once during the course of a given exper- 
iment. A user program “user()”, can be modified and downloaded quickly to support rapid devel 
opment. An on board laptop console is provided to accomplish command level communication to 
and from the user. 

Twelve 40 kHz ultrasonic sensors are provided to allow the robot to sense its environment. 
The sonar subsystem is controlled by an 8748 microcontroller. Each reading cycle takes approxi- 
mately 24 msec. Yamabico is used as a testbed for the development of MML, the model-based mo- 


bile robot language 


EXPLANATION OF FILES 


This file is a framemaker document in new_mml/mmi/macpherson/users1.doc in the 
yamabico account. , 


A. Files (n/gemini/work2/yamabico/new_mml/mml10) 

[Makefile] Unix makefile for the real-time program. 

B. Source Code files 

control.c - Tracking feedback control system of vehicle position. Each vehicle control cycle this 
module reads both optical wheel encoders and determines how to steer the vehicle based upon the 


current path element and the vehicle’s odometry estimate. 


cst.h - cubic spiral lookup table (angle [degree] vs. length [cm]). 
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geom.c - geometric and temporal functions (“set_rob”, timer etc.). 
init.s - initialize hardware, exceptions. 


interrupt.s - Interrupt handler for sonar, timer. This assembly language module is the MML 
scheduler for the multi-tasking system. 


intersection.c - Computes the intersection of sequential path elements in real time. 
immediate.c - Immediate locomotion functions that change global locomotion variables. 
main.c - main program is linked to “user.c” for real-time execution and for simulation. 
Makefile - Makes the kernel and user objects in the UNIX environment. 

math.s - Assembly code floating point mathematical functions. 

mmil.h - MML header file for external variables and constants. 

motor.s - Assembly code for drive DC motors. 

rosyio.asm.s - Assembly code for conversion of internal codes to ASCII etc. 

rosyio.c - input and output through console terminal. 

sequential.c - Sequential locomotion functions that load the instruction buffer. 
sonar.c - Functions on sonars. Described further in section XXX. 

track.c - real-time calculation of reference data as posture, velocity etc. 
leaving-point.c - Calculates the leaving point between sequential path elements. 


user.c - An application program written by a user. This file consists of the user.c commands to the 
robot. 


C. Object modules 


The object modules are the actual executable machine code that is downloaded to Yamabico. 


kernel* - system kernel (load address 304000 ~ 30d000). 


user - (load address 330000 ~). 


D. Application Programs 
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sonartest3.out - allows user to test all sixteen sonar transducers in groups of four. To load the 
program type lo=dload sonartest.out, when the program is loaded type “g 304000" to start it run- 
ning, then select 0,1,2, or 3 to indicate which logical sonar group you desire to test. Logical group 
0 consists of sonars 0,2,5, and 7; group 1 of sonars 1, 3, 4 and 6; group2 of sonars 8, 9, 10, and 11; 
and groups 3 is a “virtual” group which consists of four permanent test values. The program dis- 
plays to the screen the detected range to the four transducer in the group, type ¢ to exit or 0,1,2,or 


3 to select another group of sonars. 


ROBOT OPERATING INSTRUCTIONS 
A. Compiling a program 


[Compiling procedure for the real-time program] 

(1) Establishing the Yamabico environment. 

Login the yamabico account. 

Execute “dm9” and go to the “mml9” directory. 

The prompt “[mm19}:” will be displayed on the screen. 
(2) Create or modify “user.c” by the editor. 


(3) Key in “mu” (alias for “make user’’). 
Makefile takes care of all the system files modified. 


(4) If the message “‘kernel relented at 0x00304000” is output on the screen, the kernel should be 
downloaded at the later stage. 

Otherwise only user file has been edited and needs to be recompiled. 

See (2) of [Download the kernel & user]. 

B. Robot setup procedures 

(1) Turn on the power switch in the front panel of Yamabico leftside down. 

(2) Confirm the voltage of battery, which should be indicating between 23 to 30 volts. If the voltage 
is below 23 volts, recharge the robot’s battery. See ROBOT BATTERY AND POWER SUPPLY 
PROCEDURES below 

(3) Connect the serial port cable (blue connector) from Sun3, the baud rate is 9600 bps. 


(4) Press the reset button at the top of Yamabico. 
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C. Execution of users program 


(1) Download the kernel & user 


(a) Via the serial port, the kernel and/or the user program is downloaded. When “7920BUG>” is 
displayed on the console of Yamabico, key in “lo=dluk” + <cr> then “dluk” will be echoed back 
and the system will start to download the kernel and user. 


(b) If other messages are displayed or no response, press the reset 

button on the VME board (the reset button on top of the robot does the same function) and try 
again, or check the cable. If you want to load only user program, key in “lo=dlu”. It is important, 
for it takes about 6 minutes to download the kernel, but 30 seconds for user, so if you modified only 
user program, it is better to load only user program. 


(2) Execution of the user program 


Type “g 304000" + <cr>, then the user program will start to execute. The same program stays in 
the robot’s memory and can be restarted with the same command. 


(Cabling Instructions] 


(a) Blue 4 pin connector(RS232) is for program load and data upload. 

(b) Black many-pin connector is terminal hookup. 

(c) An additional program load cable is located in the floor by the door to Sp 511 Lab. This allows 
the programmer to download programs while Yamabico is in the hallway. 


(Placing the robot up on the blocks] 


(a) Place Yamabico in the desired position. 

(b) Carefully lift the front of the robot and place the wooden block underneath. 

(c) Carefully lift the back of the robot and place the wooden block underneath. Ensure the robot’s 
two large drive wheels do not touch the ground. Get some assistance if possible. 


D. Laptop procedures 


(1) The black, many-pin connector is terminal hookup. This can be connected to the vt220 terminal 
on the tabletop to the right of Yamabico or can be directly connected to the laptop terminal on top 
of Yamabico. The MacIntosh Powerbook145 is the current laptop terminal. Please read the 
owner’s manual prior to operating this computer. A Voice Navigator voice interface system is also 
available for voice recognition experiments. 

(2) When “7920BUG>” is displayed on the laptop screen this means that the terminal is 
functioning normally. 

(3) If "7920BUG>" is not displayed, the user must set up the terminal by starting the 
communications software package. Consult the software manual for further guidance. 
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E. Battery charging and power supply procedures 


ROBOT OFF 

(1) The MAIN SW circuit breaker on the Robot power panel is OFF. 
(2) The BATTERY circuit breaker on the Robot power panel is OFF. 
(3) The External Power Supply is switched OFF. 

(4) The Battery Charger is unplugged. 

(5) The Battery Charger connector is disconnected from the Robot. 


ROBOT OFF, CHARGING BATTERIES 

(1) Make sure the BATTERY circuit breaker on the Robot power panel is OFF. 
(2) Connect the Battery Charger output connector to the robot. 

(3) Plug the Battery Charger into the AC outlet. 


ROBOT POWERED FROM EXTERNAL POWER 
(1) Make sure the BATTERY circuit breaker on the Robot power panel is OFF. 


(2) Connect the External Power Supply output connector to the adapter on the Robot power panel. 


(3) Turn the External Power supply ON. 

(4) Turn the MAIN SW circuit breaker on the Robot control panel ON. 

(5) You may leave the Battery Charger connected and operating. The Robot will not load the 
batteries. 


SWITCHING THE ROBOT FROM EXTERNAL POWER TO BATTERY POWER 

(1) Unplug the Battery Charger from the AC outlet. 

(2) Disconnect the Battery Charger output connector from the Robot. 

(3) Turn the BATTERY circuit breaker on the Robot power panel to ON. 

(4) Turn the External Power Supply OFF. 

(5) Disconnect the External Power Supply output connector from the adapter on the Robot power 
‘panel. 


SWITCHING THE ROBOT FROM BATTERY POWER TO EXTERNAL POWER 


(1) Connect the External Power Supply output connector to the adapter on the Robot power panel. 


(2) Turn the External Power supply ON. 

(3) Turn the BATTERY circuit breaker on the Robot power panel to OFF. 
(4) Connect the : ..ttery Charger output connector to the robot. 

(5) Plug the Battery Charger into the AC outlet. 


ROBOT SIMULATION PROGRAM 
A. Files in Simulator 


yam_sim - The graphic simulator menu for selecting simulator top level commands. 
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sim - The executable simulator produced by the makefile. The compile menu button updates sim 
and the run button runs the sim. 


sim_info - Output of reference posture by the simulation program. 
axis_data - Data for “gnuplot” program. 
B. Operations for the Simulation Program 


(1) Compiling procedure for the simulation program. 

(a) Login MML working directory. 

aquarius login: yamabico (You may also use pegasus for gcc compiler.) Read the file 
AAAREADME for any new simulator developments. 

(b)If the prompt is displayed on the screen “yamabico@aquarius®”, key in “ys”. 

If logged in MML working directory, the prompt “(mml]:” is displayed on the screen. 


(c) Create or modify “user.c” by the using the EDIT button which invokes the “vi” editor. If you 
desire to use “emacs” or some other editor, then open a separate window and edit “user.c” using 
the editor of your choice. 


(d) When you have finished editing the user.c file, save the file and compile it by clicking the left 
mouse button on the CMPL button on the command menu. This executes the command make sim 
which cause the Makefile to recompile all files in the simulator that have been edited including 
user.c. 


(2) Execution of the simulation program 
(a) Click the left mouse button on the RUN button and the simulation program will start. 


(b) At first, the program will fill the instruction buffer and then the graphics portion of the program 
will display the robot trajectory on the fifth floor of Spanagel Hall. 


(c) When the simulation starts, the program will display the message and the instruction stack. 
For example, simulation output: 


Instruction Stack: 

Class argr1 argr2 argr3 argr4 argrS argr6 model mode2 

SET_ROB 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0 0 
STOP 147.717624 0.000004 69.098301 95.105652 1.884956 0.000000 0 0 
SET_ROB 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0 0 
STOP 125.656116 0.000006 58.778525 80.901699 1.884956 0.000000 0 0 


Total Number of Instruction : 4 
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(4) When the program has finished, gnuplot is automatically called to plot the robot’s whole 
trajectory to the screen or to the “ss1” printer. The robot’s trajectory information is stored in the 
axis_data file that is built as the robot simulator runs. 


PROGRAMMER GUIDE AND EXAMPLES 


Motivation 
The technique of path specification control is a fundamentally new way of specifying Yam- 


abico motion commands. Previously, all robot motion commands were specified as configurations 
consisting of p = (x, y, q). In other words, the robot was commanded to move from point to point 
with the requirement that it attain the orientation q at each specified point. This technique has been 
shown to lead to difficulties when an odometry correction was imposed upon the robot while it was 
in motion. These difficulties included reverse motion during the set_rob0 function using the con- 
figuration to configuration tracking method and non-smooth motion, resulting in wheel slippage 
that increased odometry error since the vehicle was accelerated to a higher speed until the new con- 
figuration was obtained. 

Early experimental work on robot odometry correction and wall following revealed that the 
set_rob0 function frequently caused jerky, non-smooth vehicle motion. This problem was particu- 
larly prevalent when the new odometry estimate fell behind the robot. An odometry reset to a po- 
sition behind the vehicle caused the vehicle to back up to attain the correct configuration on the 
Cartesian plane. The vehicle also was programmed to accelerate to a higher speed than the current 
operating speed in case the correction required the vehicle to “catch up” to the correction configu- 
ration. This acceleration caused increased vehicle wheel slippage that resulted in increased odom- 
etry error. 

A better way to specify robot motion is by a series of planar path elements that are connected 
to obtain the robot’s desired path. These planar elements can be straight lines, arcs (constant cur- 
vature portions of a circle), or parabolic line segments. In this way, the vehicle odometry reset is 
performed with respect to a planar path vice a single odometry configuration. This allows the robat 
to smoothly return to the specified path when the odometry estimate is reset using set_rob0. No 
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change in speed is required so the overall vehicle wheel slippage is reduced. This path specification 
control modification has required some significant modifications to the MML language. 


PROBLEM STATEMENT AND GOALS 
This section describes the problems that the new version of MML path tracking functions in- 


tend to solve. The design goals and the system constraints are explicitly stated. 


Initial Problem Statement 
The purpose of the Yamabico tracking control system is to allow the Yamabico robot to follow 


a path specified by configurations, lines, circular arcs and parabolas. The robot must automatically 
determine the transition point from one path element to the next. This includes recovery from con- 
secutive non-intersecting path elements. Additionally, the robot motion must be smooth. No dis- 
continuity in the robot’s motion is allowed. Since the absolute value of dx/ds is always finite, the 
robot’s path curvature (x) is continuous with respect to the distance travelled (s). 

Robot odometry correction is improved by performing moving odometry correction while the 
vehicle is in motion following a path. Overall vehicle motion is enhanced by using a function to 


command the vehicle to follow a parabolic path for obstacle avoidance. 


Goals 
The primary goal of the vehicle path tracking technique is to achieve smooth robot motion. 


This is accomplished by commanding the robot to follow straight, curved and parabolic line seg- 
ments as elements of the path. 

Automated path tracking includes automatic path element to path element transitions. Robust 
robot path tracking including exception handling and intelligent error recovery are built into MML 


to make robot programming easier. 


Constraints 
Physical limitations on robot motion are a characteristic of the vehicle’s size, shape and 


weight distribution. We must design to prevent such things as very sharp curvature at high speed. 
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Time is a critical factor in the sense that a finite amount of CPU time is required for the robot 
to calculate the current path image and determine the best g and x for each step. This new computer 
will perform Yamabico’s computations approximately 25 times faster than the Sun3 board. 

Robot Main memory size is another limitation, currently the robots main memory is five 
megabytes. The Sparc4 single board computer has 16 megabytes. 

Path to path transition planning must take a reasonable amount of time. Successive refinement 
of the optimum leaving point is used to ensure the best possible transition point is available at any 


given time. The first rough approximation is the intersection point, next is the transition point with- 


in one S,, of the optimum leaving point and finally the final refined optimum leaving point, deter- 


mined within one eighth of s,. An obvious trade-off exists between the time spent planning and 


the accuracy of the outcome. 
The number of Central Processing Unit (CPU) interrupt levels is limited to eight. This limits 
the number of separate tasks that can run at different levels in the single CPU, multitasking system. 


VEHICLE KINEMATICS 


Linear Vehicle Motion Control 
The Yamabico-11 mobile robot is a power wheeled steering robot [Kanayama 91]. This 


means that each drive wheel has its own independent motor. Steering is accomplished by variation 
in the relative speeds of these two motors. Vehicle linear velocity control is provided by the trap- 
ezoidal speed versus distance profile shown in Figure A.1. The velocity control algorithm for the 
vehicle is based on ramped, linear acceleration to a constant nominal velocity (vel_g). To move in 
a Straight line to a given point in the work space, the vehicle accelerates at a constant nominal ac- 
celeration rate as in region I in Figure A.1. MML provides a function for the user to change the 
nominal acceleration rate. Once the vehicle reaches the nominal velocity value the vehicle main- 
tains a constant velocity as in region Il of Figure A.1. The vehicle maintains this constant velocity 
unless it is commanded to stop at a particular point by a backward line (bline) or stop function. In 
this case the vehicle automatically calculates its distance to the stopping point. When Equation 


A.1 holds then the vehicle decelerates at a constant rate where d is the remaining path distance to 
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the stopping point, a is the robot’s nominal accleration rate, and v is the robot's current velocity. 
This is region III in Figure A.1. In region IV of Figure A.1 the vehicle is stopped. 


2ad <v2 A.l 


The required velocity of the left (vz) and right (vg) wheels are calculated by Equations A.2 
and A.3. 


vp, = (-We)y as 


Vp = (1+ Wx)v A3 


Linear Velocity Control Regions 


i 
I 





Figure A.1 Vehicle Speed Control 
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Once the desired linear velocity is determined, the vehicle image on the intended path element 
is calculated. The nature of the image calculation depends on the path element type. The value of 


the instantaneous path curvature, K, is determined using the value of the size constant 5). Based 


upon the Equations A.2 and A.3 the left (v,) and right (vp) wheel velocities are calculated, where 
W is the distance between the wheels and « is the vehicle’s instantaneous path curvature. The de- 
sired left and right wheel velocities are converted into an appropriate pulse width modulation 
(pwm) value using an empirically derived approximation function. The pwm values for the left and 
right wheels are stored in a 16 bit motor control word (mcw). This mcw is sent as a command to the 


left and right wheel motors every vehicle control cycle. 


Rotational Velocity Control Regions 


11) 
(rads/sec) 


(radians) 





Figure A.2 Vehicle Rotational Speed Control 


Rotational Velocity Control 
Vehicle rotational velocity control can be initiated only when the vehicle is in a stopped 


state. To begin rotation the vehicle must be in the stopped state in order to prevent possible wear 


or damage to the robot’s drive train gears. The power wheel steered robot has a translational motion 
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state and a rotational motion state. The method of speed control is the same as for linear velocity 
control. The speed control is illustrated in Figure A.2. The rotational velocity control description 


is provided for completeness. The robot’s current rotational velocity w_is calculated each vehicle 
control cycle by the odometry software. In region V, the robot starts out a with @. = 0 and ac- 
celerates to the nominal rotational velocity @, ,. In region VI, the robot rotates at a constant an- 
gular velocity until the vehicle’s orientation approaches the goal orientation 8 e° When the robot 


orientation satisfies Equation A.4 , the vehicle enters region VII and begins a ramped decrease of 
the current rotational velocity 8 where agg; is the robot’s current nominal rotational accleration. 
w <2a 
c 


rope) A4 


In region VIII, the robot’s current rotational velocity is reduced to zero when it reaches 6 ‘ and 


the robot’s goes back to the “stopped” state, ready to perform more rotations or translation motion 


functions. 


VEHICLE CONTROL ALGORITHM 
The method of low level vehicle control is described in this section. The MML scheduler is 


an assembly language routine that calls the appropriate interrupt handler based upon a serial board 
timer or an external interrupt to the main central processing unit (CPU). Figure A.3 illustrates the 
MML task scheduler. This section gives a detailed description of the locomotion task which is ex- 
ecuted every vehicle control cycle (100 Hertz). The high level locomotion task algorithm is given 
in Figure A.4 . This algorithm runs every vehicle control cycle. Each step of this algorithm is de- 


scribed in the rest of this section. 


Vehicle Odometry 
The vehicle odometry function determines a new dead reckoning configuration for the 


vehicle during each vehicle control cycle. This information is required to determine the proper 
steering commands for the vehicle. The distance each wheel has moved during the last vehicle con- 


trol cycle is computed by reading the storage register for each of the optical wheel encoders and 
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locomotion input/output 
task task 





Figure A.3 | MML Scheduler 


locomotion task() 


{ 


perform vehicle odometry 


store location trace data 

calculate commanded vehicle velocity 

calculate commanded vehicle image and kappa 

calculate vehicle v; and vp based upon kappa and velocity 


calculate pwm and mcw 


return pwm, mcw 





Figure A.4 Locomotion Task Algorithm 


multiplying this value by an encoder-to-distance conversion factor. The wheel encoders record a 
full wheel rotation in 512 discrete steps. This value is then filtered using a recursive digital filter 
(Hamming 83]. 
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The vehicle’s instantaneous change in orientation, A® is computed by the Equation A.5 


As RAS, 
W A.5 


A6 = 
where As is the signed incremental distance each wheel moved as determined by reading each 
wheel encoder and W is the vehicle’s effective tread width. The vehicle’s instantaneous distance 
traveled, As is computed by taking the average of the incremental distance traveled by the left and 


right wheels. The value of As is determined as shown in Equation A.6. 


As R + As 
gn ee A6 
2.0 


The vehicle’s current translational velocity (v,) and rotational velocity (@,) are determined 


next by the Equations A.7 and A.8 where At is the time interval between vehicle control cycles. 


As 
Vo = At A.7 
A® 
arena A.8 
Oe At 


Finally, the vehicle’s current dead reckoning configuration q, = (x), y;, t;, k;) is computed by 
the next function, using the vehicle’s last dead reckoning configuration gg = (xg, Yo, to, Ko) where 


Equations A.9, A.10, A.11, and A.12 are used to compute the new robot configuration. 


A®@ 
x, = X9 + As (cos6 + =) A.9 


, A@ 
y, = Yo + As (sin® + z) A.10 
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Kk = kappa A.12 


Vehicle Location Trace 
The vehicle’s current DR configuration can be stored in the robot’s on board memory 


using the location trace feature of MML. This is an user controlled option. If the location trace flag 
is switched on by the user, the vehicle’s current odometry configuration is written to the robot’s 
memory every n vehicle control cycles. The n value is user selectable. The location_trace_dump 
function allows the user to send the stored robot trajectory data back to the host computer at any 


point in the program [Sherfey 91]. 


Vehicle Commanded Velocity 
For translation motion, the commanded velocity for the left and right wheels is deter- 


mined next. First the vehicle’s current DR configuration and current path element are compared. 
A vehicle image configuration is calculated by projecting the vehicle configuration onto the current 
path element [Alexander 93][Abresch 92]. Next the commanded instantaneous vehicle path curva- 
ture (K) is computed using the image, vehicle configuration, and current path element. Finally, the 


vehicle’s commanded rotational velocity (@ .) is computed by the Equation A.13. 


o. = KXv, A.13 


The commanded left and right wheel velocities are computed by the Equations A.14 and 


A.15 respectively. 


A.14 
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n| = 


A.15 


Determination of Pulse Width Modulation (pwm) Values 
The required pulse width modulation (pwm) values for the left and right wheels are de- 


termined based upon the commanded wheel velocities according to the Equations A.16 and A.17. 


Pwmi er, = pwmlookup (v,) +kpwb(v, — vor) A.16 


PWM ight = pwmlookup (vp) +kpwb (vp - Vcr) A.17 


The pwm_lookup function returns the empirically derived pwm value for the corresponding com- 
manded wheel velocity. The motor control word (mcw) is determined based upon the vehicle’s 
commanded move direction. Normally, this is forward, but during rotation, sharp turns, and back- 


ward motion the mcw may change to a negative value. 


PATH ELEMENTS 

We first define how paths are geometrically described in this method. A configuration q stands 
for a triple 

q=(p,q.k), A.18 


where p is a point, g an orientation, and « a curvature. For an arbitrary configuration q and a 
point p, either one of the following is said to be an element: 
line(q), parabola(p), forward_line(q), backward_line(q), configuration(q) 


A path is a sequence (€1, €»-.- €,,), where each e; is an element. The meaning of each element 
e is defined as follows (each element means a directed simple path if e is not a configuration). 


1. line(q) means a circle if x # 0 or a line if « = 0 (Figure A.5, Part (a)). This path segment does 
not have any endpoints. 


2. parabola(p) means a directed parabola determined by the focus p and the directrix g. (The cur- 
vature part k of q is ignored.) (Figure A.5, Part (b)). 
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Figure A.5 Directed Path Elements 


3. forward_line(q) means a part of element line(q). It has a start p (Figure A.5, Part (c)). 
4. backward_line(q) means a part of element line(q). It has an end p (Figure A.5, Part (d)). 


5. configuration(q) does not mean a directed path segment by itself. It must have elements which 
specifies another configuration in the previous and the following position in its path. A pair of 
configurations define a cubic spiral path segment. (Figure A.5, Part (e)) (The curvature part « 
is ignored.) 

Table A.1 shows permissible combinations for two consecutive elements in a sequence. Each 


combination is depicted in Figure A.5. 
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Table A.1 :PERMISSIBLE COMBINATIONS 


| tine | TRO TRY 


TO 
seckvard-in TED at_|_ ae 





The methods of tracking entries in the table are: 
TR: normal transition 
TRE: transition at the endpoint 
CS: cubic spiral 
-: not permissible 
The finite state machine diagram in Figure A.6 describes the allowable transitions among 


robot tracking states: 


PATH ELEMENT TRANSITIONS 
Path elements are the component parts of the robot’s intended path. A method for defining 


how paths are geometrically specified is required. A configuration q as in Equation A.19 is a three 
element vector where p = (x, y) is a point in the Cartesian plane, @ is the orientation measure coun- 
terclockwise with respect to the x-axis, and « is the path curvature. A parabola is a data structure 
with a point p which represents the parabola’s focus and a configuration q which represents the pa- 
rabola’s directrix as shown in Equation A.20. For an arbitrary configuration q and a parabola r, any 
one of the following is said to be a path element: line(q), parabola(r), forward_line(q), backward_- 
line(q), cubic(q). 


q = (p, 9, x) A.19 
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Figure A.6 - Finite State Machine for Robot Status 


r= (p,q) 
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Figure A.7 The Line Function 
A path is a sequence of path elements COR een é,,), where each e, is an element. The speci- 


fication for each element e is defined as follows: each element means a directed simple path if e is 
not a configuration. 

The line(q) function commands the vehicle to track a circular path element if k #0 or a straight 
line path element if x = 0 as shown in Figure A.7. This path segment does nivi have any endpoints. 
The vehicle tracks the commanded line unless ordered to track some other path element by a sub- 
sequent user Command. The curvature (x) of the line path element is constant along the entire path. 

The parabola(r) function commands the vehicle to track a directed parabolic path element as 
determined by the focus p and the directrix g. The curvature part (k) of q is ignored. This function 


is useful for obstacle avoidance, since the world space obstacle can be the focus of the parabola. 





Figure A.8 Forward Line Tracking 
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The forward_line(q) function is a compound command. This command tells the robot to fol- 
low a cubic spiral path to the beginning of the directed half-line formed by g. Once the robot reach- 
es the start of the half-line, it tracks this line just as the line(q). Figure A.8 (a) illustrates the vehicle 
automatically using a cubic spiral path to move from its current configuration to the path element 
q, then the vehicle switches to tracking the straight forward line. Similarly, in Figure A.8 (b), the 
vehicle uses a cubic spiral to reach the configuration g and then switches to tracking the circular 
path element specified by q. 

The backward_line(q) is similar to the forward line function except the configuration q spec- 
ifies the endpoint of the path element. The vehicle may transition to other path elements after reach- 
ing the configuration q if other path element commands follow. If no command follows then the 
vehicle stops at the end configuration of the backward line. Backward lines may be straight or cir- 
cular path elements depending on the value of « in q. Figure A.9 illustrates two backward lines. 


Stopping point or 
transition point 


(a) K = 0 





Figure A.9 Backward Line Path Elements 


In Figure A.9 (a), the vehicle tracks the straight line specified by g and stops (or transitions to an- 
other path element) at the configuration qg. tn Figure A.9 (b), the robot tracks the circular path q and 
also stops at the point (q.x, q.y). 

The cubic(q) function commands the vehicle to move through a specific configuration using 
a cubic spiral path element. The cubic spiral must have a starting configuration in order to be mean- 
ingful. The command uses path elements which specify another configuration in the previous and 
the following position on its path to form the cubic spiral. When cubic(q) is the first command, the 


robot’s current configuration is used as the starting configuration for the spiral. In all cases, a pair 
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of configurations define a cubic spiral path segment. Figure A.10 illustrates the vehicle moving 


P 


inflection point a 


Figure A.10 Cubic Spiral Path Elements 





from its current configuration p to g using a cubic spiral path element. The curvature part of the 
configuration q is not used. Notice that two cubic spirals are actually used with a point of inflection 
between them. 


PATH ELEMENTS TRANSITIONS 
This section describes the allowed path element transitions in greater detail. The path element 


segments are designed to allow an autonomous robotic vehicle to follow a Voronoi path through 
an obstacle field. To accomplish this, straight lines, circular arcs, and parabolic line segments are 
required. 

Thus far, only path elements that share a common intersection point have been considered. 
Some applications require vehicle transitions between non-intersecting path elements. One simple 
example of this is lane changing for obstacle avoidance. Just as automobiles move from one paral- 
lel lane to another to avoid slow traffic or an obstacle in the road, the robot “changes lanes” from 
its current path to a parallel path (either left or right) when necessary to avoid an obstacle. Another 
application involves motion planning by representing all obstacles as circles as described in [Brutz- 
man 92). Additionally, line-to-circle, circle-to-line, circle-to-circle, parabola-to-line and line-to- 
parabola transitions are needed to enhance user program robustness and flexibility. In all of the fol- 
lowing transition descriptions, p/ is the current vehicle path element and p2 is the next path ele- 


ment. 


September 2, 1993 Page 214 














Yamabico User’s Manual Naval Postgraduate School 


Straight Line Path Transitions with Intersecting Paths 
When two consecutive directed lines, q7 and > are given, the vehicle leaves the current 


path element q; at a point p, upstream of the intersection point p;5. The optimum leaving point P, 
must satisfy the condition that the trajectory does not oscillate if it leaves g) at p, and oscillates if 
it leaves any point closer to p;> than P, ; see Figure A.11. The distance between P, and p72 is 
called the leaving distance and is proportional to So. 

For intersecting straight line paths, the intersection point is first determined. Then leaving 
points on P; are selected based upon the intersection point. From the hypothetical leaving points, 
the robot’s path is projected from path element q; to path element qg2. A non-oscillating, smooth 
transition from the first path to the second path is sought. A step value of s, is used for this process. 
The first leaving point examined is at a distance of one s,, from the intersection point on q,. The 
algorithm steps in 5, increments away from the intersection point along p; until the best leaving 
point is approximated. The best leaving point then is determined to the nearest one eighth of s,,. In 
Figure A.11, the optimum leaving point is P;. If the vehicle leaves path g; too early (for instance 
at point P,), there is less control over the robot’s motion. If the robot leaves path q; too late, for 
instance at point P,,, then the robot overshoots the intended path q> during the transition. The leav- 

‘ing distance in Figure A.11 is the distance along the path element g,; between point P; and point 
Pj. In general, the leaving distance is an increasing function of the difference in the @ values of 


the two paths at the intersection. 


Straight Line Path Transitions with Non-Intersecting Paths 
Parallel straight line path elements have no intersection point. A method of leaving the 


current path and tracking the next path must be specified in this case. When the line command is 
issued and the vehicle’s current path element does not intersect the next path element, the vehicle 
immediately stops following the current straight line path element and tracks the next parallel ele- 
ment. In other words, the vehicle’s image on the current path element is the leaving point to tran- 


sition to the new path element. 
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Figure A.11 Transition Between Intersecting Straight Line Paths 


In Figure A.12 (a) parallel paths in the same direction give simple lane changing behav- 
ior. In this example, the vehicle immediately stops tracking path element p; and lane changes to 


the left to path element pz upon receipt of the command line(&p2). 


vehicle 





Figure A.12 Transitions Between Parallel Straight Line Paths 
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In Figure A.12 (b), parallel paths with opposite orientation cause the vehicle to turn 
around. Initially the vehicle tracks path element p;. When the command line(&p2) is received, the 
vehicle immediately leaves path element p; and tracks path element p> since there is no intersec- 
tion point. Since p; and pz have opposite orientations, the vehicle turns towards path element pz 
and eventually turns all the way around to track p>. 

In Figure A.12 (c), co-linear paths cause the vehicle motion to be unchanged. Since path 
elements p; and p> have the same orientation and are collinear, no net change in vehicle motion 
occurs when the command line(&p2) is received. If pz has the opposite orientation of p; then the 


vehicle immediately turns around and follows pp. 


Straight Line to Circular Path Transitions 
This section describes transitions between line type path elements where one of the two 


elements is a circular path. The intersection of the two path elements must be considered. To de- 
termine how the path elements intersect comparison of the circular path element’s radius r and the 


minimum distance d from the line path element to the center of the circular path element must be 
made. The value of ris simply r = 1/« , where x is the curvature of the circular path. The value 
of d is determined by Equation A.21 [Kanayama 91]. For a directed line L = (a, b, 8) and a point 


p = (x, y), where p is the center of the circular path element this distance is given by; 


dist(L,p) = (y+b) cos6 — (x-a) sin@ = d A.21 


When the line and circle intersect, there are several possibilities. When the circle’s radius 
r is equal to the minimum distance d from the line to the center of the circle, the single point of 
tangency is called the osculating point. This is the simplest case since the osculating point is de- 
fined as the intersection point and is also the leaving point. This rule results in a small amount of 
vehicle path oscillation during the transition. 

When the circular path element’s radius r is less than the minimum distance dist from the 
line to the center of the circle, there are two intersection points, the upstream point and the down- 


stream point. The upstream intersection point [Alexander 93] is returned as the intersection point 
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vehicle 





Figure A.13 Transitions Between Intersecting Straight Line Paths and Circular Paths 


when the first path p; is a line and the vehicle is outside of the circle. See Figure A.13 (a). The 
leaving point is calculated by the path projection method and lies on the current path element. 
When the vehicle is on path element pg inside of the circular path element ps and the com- 
mand is read, in order to calculate a straight line to circular path transition, the downstream point 
is returned as the intersection point. The leaving point is calculated by the path projection method 
[Alexander 93]. This is shown in Figure A.13 (b). In this case, the leaving point is inside of the 


circular path element ps on path element pg. 
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Straight Line to Circular Path Transitions of Non-Intersecting Paths 
If d is greater than r, the line and circle path elements are non-intersecting. The mode of 


the circle is an important consideration when the line and circle are non-intersecting. The transition 
is only allowed when the circle and the line have the same directionality. Notice in Figure A.14 (a) 
the circle is counterclockwise (mode +) and the line’s direction is left to right. This allows the ve- 
hicle to move such that it is not forced to rapidly change direction during the transition from path 


element p; to p>. A clockwise (mode -) p2 is not allowed since the vehicle would be forced to make 


a sharp turn to the left at the leaving point. 


vehicle 


Xcp» Yep) 


(Xcp: Yep) 


vehicle 





Figure A.14 Transitions Between Straight Line Paths and Circular Paths 


Transitions between a straight path element and a non-intersecting circular path element 


or vice versa require the determination of the closest point (x ) between the two path ele- 


cp’ Yep 
ments. This point is used as the leaving point for non-intersecting elements. In Figure A.14 (a), the 


vehicle initially tracks straight line path element p;. The command line(&p2) specifies a non-in- 
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tersecting circular path. Path element p, is specified by p, = (x), y,,6,,0). Path element 
Pz = (Xz, Y2,8,,K,) is acircle with the center at the point center y center). in accordance with 


Equations A.22 and A.23, where r is the radius of the circular path element. 


= x5—rsin (8 A.22 


x center 2) 


Yoenter = ¥2 + TCos (8,) A.23 
To calculate the closest point, the value of d must be determined. Equations A.21 and 


A.25 give the closest point (x ) on the straight line path element which is the leaving point 


for the non-intersecting line to circle transition case. 


fq 
cp = *center + 4008 (8; — 5) or 


: Tt 
Yop = Yeenter + 48in (6,- 7) A.25 


For a circle to line transition, the closest point on the circle Com Yep) given by Equations 
A.26 and A.27 is used as the leaving point. When d > 0 
= 0-5 A 
Xep = *center*t Iricos ( 1 7) 26 
t 
Yep = Yeenter IN COs (8, - 5) A.27 
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and when d <0 


x 


+ 8 
Xep = center (1 Cos (8, - 5) A.28 


t 
Yep = Yeenter * Iicos (0, = 3) so 


In Figure A.14(b), the vehicle is commanded to move from a circular path element p; to 


a straight line path element p>. The closest point is used as the leaving point in the same way. 


Circle to Circle Path Transitions 


Circle to circle path element transitions are designed to provide maximum flexibility in 
vehicle motion commands. A continuuin of proximity exists between two circles with regard to the 
distance between their centers. The mode of the two circles plays an important part in determining 
how the transitions between circles should occur. Basically, their are two classes of circle-to-circle 
transition, same mode and opposite mode. Figure A.15 illustrates four types of transitions between 
non-intersecting circles. For circles with the same mode, either ++ or - -, the transitions occurs on 
the exterior of the circles. These transitions are called same mode transitions. For circles with op- 

posite mode, either +- or -+, the transition causes the vehicle to move between circles. These tan- 


gents are called opposite mode transitions. 


Circle to Circle Path Transitions (Circles with the Same Mode) 


To determine if two circles intersect, the sum of the two circle’s radii r +12 is compared 
to the distance between the centers of the two circles d. If dis greater than r, + ry, the two circles 


are non-intersecting. This case is illustrated in Figure A.16 (a) for circles with the same mode. An 
external tangent is used as an intermediate vehicle path for this type of transition. 


If d=r, + rp, then the two circles intersect at an osculating point or point of tangency. 


This case is illustrated in Figure A.16 (b) for circles with the same mode. Once again an external 


tangent is used as an intermediate vehicle path element. 
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Figure A.15 Tangential Line Segments Between Circular Path Elements 


If Ir, - rol <d <r, + 1p, the circles intersect at two points as shown in Figure A.16 (c). 


For circles with the same mode, this transition is allowed and an external tangent is used as an in- 
termediate vehicle path element. This transition is not allowed for circles with opposite modes. 


If d=Ir, - ral, then the two circles intersect at an osculating point with the smaller circle 


inside of the larger one. This case is shown in Figure A.16 (d). The osculating point is the transition 
point for circles with the same mode. This type of transition is not allowed for circles with opposite 
modes. 


If0<d< Ir; - rol, then the two circles are non-intersecting with the smaller circle inside 


of the larger one. Since d is greater than zero, the circles are not concentric. This case is shown in 
Figure A.16 (e). In this case, for circles with the same mode, the transition point is the CP on the 
current circular path element. This type of transition is not allowed for circles with opposite modes 
and is handled in section 7. 

If 0 = d, then the two circles are concentric and non-intersecting with the smaller circle 
inside of the larger one. This case is shown in Figure A. 16 (f). In this case, for circles with the same 


mode, the transition point is the current vehicle image on the current circular path element. This 
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r++ 


(a)d>Iry +r! (b) d=Iry + 19! 


(c) Iry - gl <d<Iry +19 (d) d=Iry - rl 


(e)0<d<lr, - rl 





Figure A.16 Circle to Circle Transitions, Same Mode 


causes the vehicle to transition immediately from the first path element to the second. Once again, 


this type of transition is not allowed for circles with opposite modes. 
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Circle to Circle Path Transitions (Circles with the Opposite Mode) 
In Figure A.17 (a) non-intersecting circular path elements with opposite modes are shown. 


Notice an external tangent is used as an intermediate path element between the two circles. 


rl+- 
(a)d>Ir; — rol, Non-intersecting Circles with Opposite Modes 


rl+- 


(b) d =Ir) + rl, Circles with an Osculating Point 





Figure A.17 Transitions Between Circles with Opposite Modes 


For intersecting circles with opposite modes, the transition is illustrated in Figure A.17 (b). 


Notice the vehicle uses the osculating point as the transition point between path elements. 


Line to Parabola Path Transitions (Intersecting Paths) 
Transitions to and from parabolic path elements are only allowed from straight line path ele- 


ments due to the complex nature of circle-parabola intersections. Circle to parabola transitions may 
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have up to four intersection points for some geometries. In Figure A.18 (a), the directed parabolic 
path element p, is specified by a five element vector in accordance with Equation A.30. 


Pp (Xp YpxXa Var 8) A.30 


Where (x, yg) is the focus of the parabola and (xg, yg, 0.) is the parabola’s directrix. The 
straight line path element p, = (x), y,,8,,«,) is specified as before. The vehicle is commanded 
to follow path p, and then p2, sequentially. The intersection point between p; and pz is first cal- 


culated and then the appropriate leaving point on p, is determined [Alexander 93]. In a similiar 


(a) Parabola-to-Line Transition 


\ 


(b) Line-to-Parabola Transition 





Figure A.18 Transitions between Intersecting Line and Parabolic Path Elements 
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(a) Parabola-to-Line Transition (b) Line-to-Parabola Transition 


Figure A.19 Transitions between Non-Intersecting Line and Parabolic Path Elements 


fashion, the vehicle may be commanded to follow a straight line path p; and then transition to a 
parabolic path pz as shown in Figure A.18 (b). The intersection point and the leaving point are cal- 


culated in a similiar manner. 


Line to Parabola Path Transitions (Non-Intersecting Paths) 
For a straight line-to-parabola transition, refer to Figure A.19. If the path elements do not in- 


tersect, then the closest point on the straight line path element is used as the leaving point. Similar- 
ly, for parabola-to-straight line transitions with no intersection among path elements, the CP on the 


parabola is used as the leaving point as shown in Figure A.19 (a). 


Definitions 
Closest Point (CP) - The point on the current path element that has the shortest Euclidean dis- 


tance to the next sequential path element. This applies to non-intersecting paths only. 
Configuration - a four element data structure used to describe a robot position or a path ele- 
ment. The four elements are x, y, 8, and «. 
Immediate Function - Functions that are executed immediately upon the command interpreter 
reading them. This type of command is not held in a buffer for subsequent execution. Instead, the 


affected parameters are changed immediately. 
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Intersection Point - The point or points of intersection between two successive path elements. 
This is used as a first approximation of the transition point. 

Leaving Distance - The distance along the current path element from the leaving point to the 
intersection point. 

Parabola - a five element data structure used to describe a parabolic path element. The five 
elements are Xp Vp Xop Yop and qj. The parabola’s focus is represented by the point Op yp and the 


directrix of the parabola is the configuration (x aa We 0.0). 


Sequential Function - Functions that are executed in a sequential fashion. Each sequential 
function awaits the logical completion of the previous sequential function. 

Transition Point - (Same as leaving point) - The latest hypothetical leaving point that does not 
result in oscillation in the transition to the next path. At this point the vehicle switches from track- 


ing the current path to the next path. 


The Flow of Control 
The initialization of variables occurs first. Then control is transferred to user.c. The sequen- 


tial-type commands are placed into the command buffer and the immediate type commands are ex- 
ecuted immediately. The wait_motion and mark_motion commands are used to temporarily halt 


reading commands into the command buffer. 


‘VEHICLE MOTION COMMANDS 


Set Robot Configuration Sequential (set_rob) 


Syntax: void set_rob(q) 
CONFIGURATION gq; 


Description: 


Sets the robot’s odometry configuration in a sequential manner. This func- 
tion is used normally at the start of the MML program to tell the robot 
where it is initially. Subsequent odometry resets are also made using this 
function. This function is illustrated in Figure A.20. 
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Figure A.20 - The set_rob Function 


Function Call: set_rob(&q) 
Location: loco.c 
Temporal Type: Sequential Function 


Set Robot Configuration Immediate (set_rob0) 


Syntax: void set_rob0(q) 
CONFIGURATION gq; 


Description: 


Resets the robot’s odometry configuration such that the robot continues 
moving in a smooth manner. This resets the Xod Yod and Gog Components 


of the robot’s configuration only. The kappa of the robot cannot be reset 
since this would result in an instantaneous change in the robot’s curvature. 
This is not allowed. In Figure A.20, the vehicle is tracking a the desired 
path. In this case, set_rob0 is used to reset the robot’s odometry configu- 
ration from the current estimate esr to the actual current configuration 
Gact 

Function Call: set_rob0(&q) 

Location: loco.c 


Temporal Type: Immediate Function 


Get Robot Configuration Immediate (get_rob0) 


September 2, 1993 Page 228 








Yamabico User’s Manual Naval Postgraduate School 


Syntax: void get_rob0(q) 
CONFIGURATION q; 


Description: 


Retrieves the robot’s odometry estimate. Returns a pointer to the location 
of the robot’s current estimate of its configuration. 

Function Call: get_rob0(&q) 

Location: loco.c 

Temporal Type: Immediate Function 


Move While Tracking a Line (line) 


Syntax: void line(q) 
CONFIGURATION q; 


Description: 


Command that orders the robot to follow the line specified by the config- 
uration q. If the path curvature is zero then g.kappa = 0.0. This means that 
the path represents a straight line passing through the point (q.x, g.y) with 
orientation q.theta. If the path curvature is nonzero, then the robot follows 
a circular path. When the value of x is less than zero then the vehicle’s 
direction of motion on the circle is clockwise, and when K is greater than 
zero, then the motion is counterclockwise. These concepts are illustrated 
in Figure A.21. Speed is automatically reduced to allow the robot to make 
sharp turns. This is reflected by the dependency between K and the vehicle 
speed. In simple terms, the vehicle speed must be reduced to allow it to 
move safely with larger values of «. 


Function Call: line(&q); 
Location: loco.c 
Temporal Type: Sequential Function 


Move While Tracking a Forward Half Line (forward _line) 
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q-kappa = 0.0 (straight line) 


q-kappa < 0.0 (clockwise) 





Figure A.21 - The line Function 


Syntax: void fline(q) 
CONFIGURATION gq; 


Description: 


Follow the forward_line specified by a configuration q. If the vehicle im- 
age is on the half line specified by q, then the vehicle uses this line as its 
path. Otherwise, if the vehicle’s image does not fall on the half line (i.e. 
behind the point (¢.x, g.y)) then the vehicle shifts to configuration-to-con- 
figuration tracking using a cubic spiral path specification until the line’s 
starting point is reached. See Figure A.22. In case 1 the vehicle’s image 
falls on the half line, in this case the vehicle moves in exactly the same 
fashion as for the line function. In case 2, the vehicle’s image does not fall 
on the half line. Vehicle motion in case 2 uses point tracking with cubic 
spirals as the shape of the path. The vehicle tracks to the point (¢.x, g.y) 
and passes close to this point. The vehicle must pass through the configu- 
ration (q.x, q.y, q.f) as it transitions onto the forward half line. 


Function Call: fline(&q); 
Location: loco.c 
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Temporal Type: Sequential Function 


close to the start of the 


(q.x, q.y) 


CASE 2-Track through | CASE 1 - Forward Line Tracking 
the fline starting point vehicle 





Figure A.22 - The fline Function 


Move While Tracking a Backward Half Line (backward_line) 


Syntax: void bline(q) 
CONFIGURATION gq; 


Description: 


Follow the backward_line specified by a configuration g. See Figure A.23 
for an illustration. In case 1, the vehicle image falls on the half-line and the 
robot tracks as in the line function. In case 2, the vehicle’s image does not 
fall on the half-line, this is an undefined situation that gives an error mes- 
sage or an exception handler. The configuration point (q.x, g.y) can be used 
as a vehicle stopping point or as a transition for configuration-to-configu- 
ration tracking (see the config(q) command). 


Function Call: bline(&q); 


Location: loco.c 


September 2, 1993 Page 231 











Yamabico User’s Manual Naval Postgraduate School 


Temporal Type: Sequential Function 


CASE 1 - Line Tracking CASE 2 - Stop as Soon as 
Possible 


»- 


vehicle 





Figure A.23 - The bline Function 


Define a Robot Configuration Variable (def_configuration) 


Syntax: CONFIGURATION def_configuration(x, y, t, k, &p) 
double x; 
double y; 
double t; 
double k; 
CONFIGURATION p; 


Description: 


Assigns the four parameters necessary to specify a configuration. The pa- 
rameters x and y define the vehicle’s location on the cartesian plane. The 
parameter t represents the vehicle’s orientation and x represents k, the cur- 
vature of the vehicle’s current motion. 
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Function Call: def_configuration(x, y, t, k, &p); 
Location: geom.c 
Temporal Type: Immediate Function 


Define a Parabolic Path Variable (def_parabola) 


Syntax: PARA def_parabola(xf, yf, xd, yd, td, &p) 
double xf; 
double yf; 
double xd; 
double yd; 
double td; 
PARA p; 


Description: 


Assigns the five parameters necessary to specify a parabola. The point Op 
yp is the focus of the parabola and the configuration (x 3, ¥4 tp 0.0) is the 


directrix of the desired parabola as shown in Figure A.24. Notice the di- 
rectrix is always a straight line, therefore the directrix has k = 0 by default. 


Function Call: def_parabola(xf, yf, xd, yd, td, &p); 
Location: geom.c 
Temporal Type: Immediate Function 


Move While Tracking a Parabola (parabola) 


Syntax: void parabola(p) 
PARA p; 


Description: 
Follow the parabola specified by a focus Op yp and a directrix specified 


by a configuration (x4, Yq, tg, 0.0), see Figure A.24. The parabola function 


is used primarily as a means of obstacle avoidance. Figure A.24 illustrates 
the robot following a straight line path. When an obstacle is encountered 
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on the robot’s intended path, a shift is made to temporary parabolic path 
tracking. This allows the robot to smoothly maneuver around the detected 
obstacle and return to the intended straight line path. 


Function Call: parabola(&p); 
Location: loco.c 
Temporal Type: Sequential Function 


(xd, yd, td, 0) 


obstacle 





Figure A.24 - The para function 


Move to a Configuration (config) 


Syntax: void config(q) 


CONFIGURATION q; 


Description: 


This function specifies a configuration as a destination and uses the con- 
figuration from the previous motion commands to reach the destination us- 
ing one or two cubic spirals. The kappa value of the configuration is ig- 
nored since cubic spirals start and end with zero curvature. This is the 
move command used in previous versions of MML. In Figure A.25 the ve- 
hicle is commanded to move through three successive configurations us- 
ing a series of config commands, namely config(&q1), config(&q2), and 
config(&q3). The robot automatically plans a smooth cubic spiral path be- 
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tween successive configurations. Note that not all config-to-config 
transitions are allowed due to the nature of cubic spirals. An error message 
or an exception handler is required to recover when prohibited pairs of 
configurations are specified. 


Function Call: config(&q); 
Location: loco.c 
Temporal Type: Sequential Function 


q3 





Figure A.25 - The config function 


Immediate Stop (stop) 


Syntax: void stop0() 


Description: 


This function make robot stop dynamically near the point this function was 
issued. This is not a sequential function, but an immediate one. The se- 
quential functions that have been issued are cancelled. The vehicle accel- 
erates at negative of the set acceleration until fully stopped. All current in- 
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structions are flushed from the instruction buffer. This function is related 
to the halt0() and resume0() functions. 


Function Call: stop00; 
Location: loco.c 
Temporal Type: Immediate Function 


Halt Robot (halt0) 
Syntax: void halt0Q 


Description: 


This function make robot stop dynamically near the point this function was 
issued. This is not a sequential function, but an immediate one. The vehicle 
accelerates at negative of the set acceleration until fully stopped. It does 
not modify the instruction buffer. This function is related to the stop0() 
function and is cancelled by a subsequent resume(() function. The robot 
motion can continue when the resume0Q function is issued. 


Function Call: halt0Q; 
Location: loco.c 
Temporal Type: Immediate Function 


Location: loco.c 


Temporal Type: Immediate Function 


Resume Robot Motion (resume0) 


Syntax: void resume0() 


Description: 


This function allows the robot to move once again after a halt0() function 
has been issued. This function does not modify the instruction buffer in 
any way. The robot motion cannot be resumed until the resume0() function 
is issued. This function is related to the halt0( function. 


should be stored so they do not have to be recomputed 
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Function Call: size_const0(s); 
Location: loco.c 
Temporal Type: Immediate Function 


Set Size Constant Sequential (size_const) 


Syntax: void size_const(s) 
double s; 


Description: 


This function sequentially updates the size constant s,, for the tracking al- 
gorithm. The new value of s, is stored in the instruction buffer and s, is 
changed when the sequential command is executed. 

Function Call: size_const(s); 

Location: loco.c 

Temporal Type: Sequential Function 


Set Vehicle Speed Immediate (speed0) 


Syntax: void speed0(s) 
double s; 


Description: 


This function immediately resets tiie vehicle speed. The nominal vehicle 
speed is 30 cm/sec. The vehicle moves at the nominal speed until a new 
speed is set or reset. The vehicle smoothly accelerates to the new speed us- 
ing the value of the current vehicle acceleration value as the rate. 


Function Call: speed0(s); 
Location: loco.c 
Temporal Type: Immediate Function 
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Set Vehicle Speed Sequential (speed) 


Syntax: void speed(s) 
double s; 


Description: 


This function sequentially resets the vehicle speed. The nominal vehicle 
speed is 30 cm/sec. The vehicle moves at the nominal speed until a new 
speed is set or reset. The vehicle smoothly accelerates to the new speed us- 
ing the current vehicle acceleration value as the rate. 


Function Call: speed(s); 
Location: loco.c 
Temporal Type: Sequential Function 


Set Vehicle Acceleration Sequential (acc) 


Syntax: void acc(a) 
double a; 


Description: 


This function sequentially resets the vehicle acceleration. The nominal ve- 


hicle acceleration is 20 cmIsec2. The vehicle accelerates at the nominal ac- 
celeration until a new acceleration is set or reset. 


Function Call: ace(a); 
Location: loco.c 


Temporal Type: Sequential Function 


Set Vehicle Acceleration Sequential (acc0) 


Syntax: void acc0(a) 
double a; 
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Description: 


This function immediately resets the vehicle acceleration. The nominal ve- 


hicle acceleration is 0.5 cmIsec. The vehicle accelerates at the nominal 
acceleration until a new acceleration is set or reset. 


Function Call: acc0(a); 
Location: loco.c 
Temporal Type: Immediate Function 


Set Vehicle Rotational Speed Immediate (r_speed0) 


Syntax: void r_speed0(s) 
double s; 


Description: 


This function immediately resets the vehicle’s rotational speed. The nom- 
inal vehicle rotational speed is 0.5 rad/sec. The vehicle moves at the nom- 
inal rotational speed until a new speed is set or reset. The vehicle smoothly 
accelerates to the new speed using the value of the current vehicle rotation- 
al acceleration value as the rate. 


Function Call: r_speed0(s); 
Location: loco.c 
Temporal Type: Immediate Function 


Set Vehicle Speed Sequential (r_speed) 


Syntax: void r_speed(s) 
double s; 


Description: 


This function sequentially resets the vehicle’s rotational speed. The nom- 
inal vehicle speed is 0.5 rad/sec. The vehicle moves at the nominal rota- 
tional speed until a new rotational speed is set or reset. The vehicle 
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smoothly accelerates to the new speed using the current vehicle rotational 
acceleration value as the rate. 


Function Call: r_speed(s); 
Location: loco.c 
Temporal Type: Sequential Function 


Set Vehicle Rotational Acceleration Sequential (r_acc) 


Syntax: void r_ace(a) 
double a; 


Description: 


This function sequentially resets the vehicle’s rotational acceleration. The 


nominal vehicle acceleration is 0.5 rad/sec2. The vehicle accelerates at the 
nominal acceleration until a new acceleration is set or reset. 


Function Call: r_ace(a); 
Location: loco.c 
Temporal Type: Sequential Function 


Set Vehicle Rotational Acceleration Immediate (r_acc0) 


Syntax: void r_accO(a) 
double a; 


Description: 


This function immediately resets the vehicle’s rotational acceleration. The 


nominal vehicle acceleration is 20 rad/sec?. The vehicle accelerates at the 
ominal rotational acceleration until a new acceleration is set or reset. 


Function Call: r_acc0{a); 
Location: loco.c 
Temporal Type: Immediate Function 
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Get Total Distance Traveled (path_length) 


Syntax: double path_length() 
Description: 


Returns the robot distance traveled since the start of the current program. 
This distance is stored as the parameter ss and it is updated every 10 msec 
by the odometry function. (This is the control function in the file control.c) 
This function is a critical part of the current robot odometry correction. 


Function Call: distance = path_length(); 
Location: loco.c 
Temporal Type: Immediate Function 


Wait for a Point (wait_point) 


Syntax: void wait_point(p) 
POINT p; 


Description: 


Busy waits in task level 0 until the vehicle’s image passes a certain point. 
This function delays the stepwise reading of the “user.c" file until the dis- 
tance from the vehicle’s image to a specified point reaches a local mini- 
mum. 


Function Call: wait_point(p); 
Location: loco.c 


Temporal Type: Sequential Function 


Leave the Current Path Element (skip) 


Syntax: void skip() 
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Description: 


Causes the robot to immediately leave the current path element. The robot 
will immediately start tracking the next sequential path element if one ex- 
ists, otherwise the robot stops moving near its current position. Cannot be 
used with a rotate-to-cubic spiral transition or any transition-at-endpoint 
(TRE) command sequence. 


Function Call: skipQ; 
Location: loco.c 
Temporal Type: Immediate Function 


Get the Current Path Element (get_line) 


Syntax: PATH_ELEMENT get_line() 


Description: 


Returns the path element that the robot is currently tracking. The current 
path element is returned in the form of a PATH_ELEMENT record. This 
record consists of four fields; pc (type CONFIGURATION), pp (type 
POINT), tp (type POINT), and type (type int). In the case of an fline func- 
tion, the path element returned is a cubic spiral or an sline depending on 
the state of the compound function. 


Function Call: element = get_line(); 
Location: loco.c 
Temporal Type: Immediate Function 


MML IMPLEMENTATION DETAILS 


MML System Software Architecture 
The path tracking MML system architecture is shown in Figure A.26. This is a partial repre- 


sentation of the entire system. This diagram focuses on the vehicle locomotion. When the program 


starts to run, initialization occurs first. All global variables are given an initial value and the con- 
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stants are defined. After the initialization, control is transferred to the user.c code. This is basically 
the user’s commands for the robot. Each command calls a specific MML function. Each MML 


function is either sequential or immediate. 
Table A.2 : MML SYSTEM TASK PRIORITY 
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In the level 0 task process, sequential MML functions load the necessary path element infor- 







mation into the command buffer. For a path element function such as line(&p), the path element 
configuration p is loaded into the command buffer and the path intersection point and leaving point 
are calculated when two or more paths are pending. Since the intersection point and leaving point 
functions currently run in the foreground, there is some delay in reading sequential user commands. 
In a later version, the intersection point and leaving point calculations will be tasks run at an inter- 
rupt level above the foreground. 

Immediate MML functions change one or more global variables, but do not load information 
into the command buffer. Immediate functions change robot parameters immediately. One exam- 
ple is the speed0(sp) function. This function sets the vehicle parameter vel_c equal to sp, which 
immediately changes the current vehicle speed. Upon receipt of this command, the vehicle smooth- 
ly accelerates to the new commanded speed. 

Some explanation of the multitasking processes is required. The Motorola 68020 CPU has 
eight interrupt levels [Motorola 85]. Some of these interrupts are used to run vehicle tasks at vari- 
ous priority levels in the single CPU, multitasking system. Table A.2 illustrates these vehicle tasks. 
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Figure A.26 - MML System Architecture 
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The higher the interrupt level, the higher the priority of the associated task. At the highest level 
is the robot’s reset button, this tasks overrides all other tasks, stops the robot and resets the CPU. 
Levels five and six are currently not used. Interrupt level four is the highest priority task that runs 
during robot operations. This important task is responsible for steering the vehicle. Every 10 msec, 
the locomotion task interrupts all other lower priority running tasks and runs for about 2500 micro- 
seconds. This task first reads the shaft encoders and computes the vehicle’s odometry configuration 
estimate. This is a dead reckoning technique since only internal devices are read. Next the most 
recent odometry configuration is used to calculate the proper k and velocity for the vehicle. These 
parameters are used to determine the desired vehicle rotational velocity @. A kinematic function 


calculates the left and right wheel velocities v, and Vp. This information is used to determine the 


necessary pwm command to be sent to the left and right wheel drive motors. 

At interrupt level 3, the vehicle’s user interface input/output task runs. This task is responsible 
for printing information to the vehicle on board monitor and reading input from the user entered 
on the console’s keyboard. Also, file transfer from the robot to the host computer is controlled by 
this task. 

At interrupt level 2, the vehicle sensor functions run. This interrupt is triggered by range in- 
formation that is placed in the sonar board register. When one or more vehicle sonars are enabled, 
this task runs about every 30 msec. When none of the robot’s twelve sonars are enabled, this tasks 

does not run at all [Sherfey 91]. 

Interrupt level 1 is the msbn() task which is currently not used. Eventually, the intersection 
point and leaving point tasks will be transferred to this level. 

Interrupt level 0 is the user’s instruction interpretation task. Initialization of all variables and 
interpretation of the user’s commands run at this level. All other higher priority tasks can interrupt 
the level 0 task. This task fills the command buffer based on the user’s sequential commands and 
modifies system parameters based upon immediate commands. The sonar sensors are enabled and 


disabled at this level. All robot navigation functions run at this level also. 


The Command Buffer Data Structure 
1. The position of the transition point in the instruction buffer is such that the first path element 


is written into the instruction buffer with no transition point. The second path element is written 
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into the instruction buffer only after the transition point between the first and second path elements 
is determined. All writes to the instruction buffer are atomic. The following is an example: 

2. Pointers are maintained to positions in the buffer as follows: 

a. current_path_element_ptr - the points to the path element that the robot is currently track- 
ing. 

b. current_inst_ptr - points to the current instruction in the instruction buffer. This instruction 
is not always a path element. 

c. next_path_element_ptr - points to the next path element type instruction. This pointer is 
used in conjunction with the current_inst_ptr to calculate the transition point. It is also used by the 
transition point test routine to determine if the robot has reached the transition point. 


d. new_inst_point - Points to the next sequential empty portion of the instruction buffer. 


CONFIGURATION 


COMMAND TYPE 


LEAVING POINT 
INTERSECTION (boolean flag) 





Figure A.27 - Data Structure for the Command Buffer 


3. The control function is reconfigured to remove all functions previous done by stepper(). 


Level 4 tasks run every 10 msec in the following order: 
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a. Odometry (reads wheel encoders) 


b. v, omega update; 

1. CASE SSTOP: cur_v = 0; cur_w = 0; 

2. CASE SMOVE: current_image = update_image(vehicle, path); 

test_TP(current_image, trans_pt); 

kappa = update_kappa(vehicle, path); 

vel_c = update_vel(); 

cur_w = update_w(); (= kappa * vel_c) 

3. CASE RMOVE: cur_v = 0; cur_w = update_w() /* Trapezoidal control of rotational ve- 
locity */ 

c. update_pwm d. location trace (conditional) 

The command interpreter uses an array structure to store path element records. The interpreter 
reads the user.c file one instruction at a time. The procedure set_instr reads each path element com- 
mand, loads the path element data into a path element record and then places the record into the 
command buffer. The overall system structure is illustrated in Figure A.27. Access to this structure 
is First-In-First-Out, so commands are stored in the same order as they appear in user.c. 

Individual path element records consist of the following fields; configuration, point, com- 
mand type, leaving point and a stop flag. Not all fields are used for all types of path elements, for 
instance, for a line(&p) command, the point field is not used since the line’s configuration is stored 
in the configuration field and the point portion is not necessary. The leaving point part of the path 
element record is the point where the vehicle should stop following the current path element and 
start following the next path element. The leaving point is calculated for the first and second ele- 
ments in the command buffer only. All other leaving point calculations are held pending until the 
vehicle is actually following the first path element. 

The parabola type path element .3 one command type that uses the point part of this record. 
This portion of the record is used as the focus of the parabolic path element. The path the vehicle 
is currently following is pointed to by the current_path pointer. This is a pointer to a path element 


record that represents the vehicle’s current path. This path element record is removed from the 
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Higher Priority 


Level 4 Task 
Motor Control 





Figure A.28 - System Control Timing 


command buffer when the vehicle starts to follow the new path element. The next planned path 
then moves to the head of the buffer. Leaving point calculation is initiated every time an element 
is removed from the buffer such that the first and second path elements in the buffer have their leav- 


ing points calculated. 
b. Sonar Functions - located in the file sonar.c 


Procedure: sonar(n) 


Description: returns the distance (in centimeters) sensed by the n" ultrasonic sensor. If no echo is 
received, then a -1 is returned. If the distance is less than 10 cm, then a 0 is returned. 


Procedure: enable_sonar(n) 
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Description: enables the sonar group that contains sonar n, which causes all the sonars in that group 
to echo-range and write data to the data registers on the sonar control board. Marks the n'th position 
of the enabled_sonars array to track which sonars are enabled. 


Procedure: disable_sonar(n) 


Description: removes the sonar n from the enabled_sonars list. If sonar n is the only enabled sonar 
from its group, then the group is disabled as well and will stop echo ranging. This has benefit of 
shortening the ping interval for groups that remain enabled. 


Procedure: wait_sonar(n) 


Description: Busy waits at level 0 until new data is available for sonar n. Then returns the range 
value for sonar n. 


Procedure: global(n) 


Description: returns a structure of type POSIT containing the global x and y coordinates of the 
position of the last sonar return. 


Procedure: enable_linear_fitting(n) 


Description: causes the background system to gather data points from sonar n and form them into 
line segments as governed by the linear fitting algorithm. Increments service_flag. 


Procedure: disable_linear_fitting(n) 


Description: causes background system to cease forming line segments for sonar n. Decrements 
the service_flag. Will also disable the calculation of global coordinates for that sonar if data 
logging of global data is not enabled. 


Procedure: enable_data_logging(n,filetype,filenumber) 


Description: causes the background system to log data for sonar (n) to a file (filenumber). The data 
to be logged is specified by an integer flag (filetype). A value of 0 for filetype will cause raw sonar 
data to be saved, 1 will save global x and y, and 2 will save line segments. The filenumber may 
range between 0 and 3 for each of the three types, providing up to 12 data files. Example: 
enable_data_logging(4,1,0); 
will cause raw data from sonar #4 to be saved to file 0, while: 
enable_data_logging(7,2,0); 
will cause segments for sonar #7 to be saved to file 0. 
Function increments the service_flag. 
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Procedure: disable_data_logging(n,filetype) 


Description: causes the background system to cease logging data of a given filetype for a sonar n. 
Decrements the service_flag. 


Procedure:serve_sonar(x, y, t, ovfl, datal, data2, data3, data4, group) 


Description: this procedure is the “central command” for the control of all sonar related functions. 
It is linked with the ih_sonar routine and loads sonar data to the sonar_table from there. It then 
examines the various control flags in the sonar_table to determine which activities the user wishes 
to take place, and calls the appropriate functions. This procedure is invoked approximateiy every 
thirty milliseconds by an interrupt from the sonar control board. 


Procedure: get_segment(n) 

Description: returns a pointer to the oldest segment on the linked list of segments for sonar n; i.e. 
the record at the head of the linked list. It is destructive, thus subsequent calls will return 
subsequent segments until the list is empty. This is accomplished by first copying the contents of 
the head record into a temporary record called segstruct and then freeing the allocated memory for 
the head record. The pointer returned is actually a pointer to this temporary storage. If get_segment 
is called on an empty list a null pointer is returned. 

Procedure: get_current_segment(n) 

Description: retums a pointer to the segment currently under construction if there is one, otherwise 
returns null pointer. This is accomplished by calling end_segment, copying the data into segstruct 
and then returning a pointer to segstruct. The memory allocated by end_segment is then freed. 
Procedure: set_parameters(cl,c2,c3) 

Description: allows the user to adjust constants which control the linear fitting algorithm. C1 is a 
multiplier for standard deviation and C2 is an absolute value; both are used to determine if an 
individual data point is usable for the algorithm. C3 is a value for ellipse thinness; it is used to 
determine the end of a segment. Default values are set in main.c to 3.0, 5.0, and 0.1 respectively. 
Procedure: enable_interrupt_operation() 

Description: places sonar control board in interrupt driven mode. 


Procedure: disable_interrupt_operation() 


Description: stops interrupt generation by the sonar control board. A flag is set in the status register 
when data is ready, and it is the user's responsibility to poll the sonar system for the flag. 
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Procedure: calculate_global(n) 


Description: this procedure calculates the global x and y coordinates for the range value and robot 
configuration in the sonar table. The results are stored in the sonar table. 


Procedure: linear_fitting(n) 


Description: this procedure controls the fitting of range data to straight line segments. First it 
collects three data points and establishes a line segment with it's interim data values. After the 
segment is established, the procedure tests each subsequent data point to determine if it falls within 
acceptable bounds before calling the least squares routine to include the data point in the line 
segment. After inclusion of the data point the segment is again tested to ensure the entire set of data 
points are sufficiently linear. If any of the tests fail, the line segment is ended and a new one started. 
The completed line segment is stored in a data structure called segment, and segments are linked 
together in a linked list. 


Procedure: start_segment(n) 

Description: this procedure establishes a new line segment with the three data points contained in 
segment_data[n].init(x and y). It writes the appropriate data to the interim values in 
segment_data[n]. 


Procedure: add_to_line(n, x, y) 


Description: this procedure calculates new interim data for the line segment and stores it in 
segment_data[n]. It also changes the end point values to the point being added. 


Procedure: end_segment(n) 


Description: this procedure allocates memory for the segment data structure, loads the correct 


values into it and returns a pointer to the structure. 
Procedure: reset_accumulators(n); 


Description: resets the accumulative values in segment_data[n] (sgmx, sgmy, sgmx2, sgmy?, 
sgmxy) to zero. 


Procedure: build_list(ptr, n); 


Description: this function accepts a pointer to a segment data structure and a sonar number, and 
appends the segment structure to the tail of a linked list of structures for that sonar. 


Procedure: log_data(n, type, filenumber,i) 
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Description: this procedure causes data to be written to a file. The filenumber designates which 
“column” (0,1,2, or 3) of a two dimensional array for that type of data is used. The data array and 
a counter for each column forms the data structure for each type. The value of i is used to index the 
seg_list array for storing line segments. 


Procedure: set_log_interval(n, d) 


Description: this procedure allows the user to set how often the sonar system writes data to the raw 
data or global data files. The interval d is stored at sonar_table{n], and one data point will be 
recorded for every d data points sensed by the sonar. Default value for interval d is 13, which for 
a speed of 30 cm/sec and sonar sampling time of 25 msec should record a data point every 10 cm. 


Procedure: wait_until(variable, relation, value) 


Description: this procedure will delay it's completion (and thus the continuance of the program it's 
embedded in) until the variable achieves the relation with the value specified. For example, 
presume the robot is traveling along the x axis. If the user wants the robot to begin recording sonar 
data when the x position of the robot exceeds 500 cm., he would insert this command after the 
move command: 

wait_until(X,GT,500.0); 

enable_sonar(sonar number); 


The variable are predefined as X, Y, A and DO through D1], and correspond to the robot's current 
X position, y position, theta, and range from sonars 0 through 11. Relations are predefined as GT, 
LT and EQ corresponding to greater than, less than and equal to. Value may be any number 
expressed as a double or the predefined values PI, HPI, P1I34, P14, or DPI. 


Procedure: xfer_raw_to_host(filenumber, filename) 


Description: this function allocates memory for a buffer and then converts a raw data log file to a 
string format stored in the buffer. It then calls host_xfer to send the string to the host. When that 
transfer is complete, it frees the memory it allocated for the buffer. Filename must be entered in 
double quotes 

(“dumpraw” for example). 


Procedure: xfer_global_to_host(filenumber, filename) 


Description: this function performs the same function as xfer_raw_to_host, except it transfers 
global data vice raw data. 


Procedure: xfer_segment_to_host(filenumber, filename) 
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Description: this function performs the same function as xfer_raw_to_host, except it transfers 
segment data vice raw data. 


Procedure: finish_segments(n) 


Description: this function completes segments at the end of a data run. Necessary because the linear 
fitting function only terminates a segment based on the data - it has no way of knowing that the 
user has stopped collecting data. 


b. Programming Examples 





These user.c file is provided as simple examples of robot programs written in MML 
Example: The first example is a simple racetrack. The user.c file follows 


#include “mml.h” 


user () 
{ 
CONFIGURATION pstart; 
CONFIGURATION first_path; 
CONFIGURATION second_path; 
CONFIGURATION third_path; 
CONFIGURATION fourth_path; 
CONFIGURATION fifth_path; 
double s = 10.0; 
int laps; 
int lap_count = 0; 


buffer_loc=index_loc=malloc (300000) ; 
bufloc=indxloc=(double *)malloc(60000); 
loc_tron(2,0x3f,30); 


def_configuration(1200.0, 65.0, 0.0, 0.0, &pstart); 
def_configuration(1100.0, 65.1, 0.0, 0.0, &first_path); 
def_configuration(1500.0, 65.0, 0.0, 0.02, &second_path) ; 
def_configuration(1700.0, 164.9, PI, 0.0, &third_path); 
def_configuration(1200.0, 165.0, PI, 0.02, &fourth_path); 


set_rob(&pstart) ; 

size_const(s); 

speed (15.0); 

xr_printf("\12 Enter desired number of laps. "); 
laps=getint (CONSOLE) ; 
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while (lap_count < laps) 


{ 

line (&first_path) ; 

line (&second_path) ; 

line (&third_path) ; 

line (&fourth_path) ; 

++lap_ count; 

} /* end while loop */ 
line (&first_path) ; 
wait_until(xX, GT, 1400.0); 
loc_troff(); 
halt(); 
motor_on = NO; 
loc_trdump (“loc_dump. 8Dec92") ; 

} /* end user.c */ 


A plot of the robot’s motion is shown in Figure A.29. The user first declares five configura- 
tions and other variables needed for the program. Next the location trace function is enabled. Then 
the configurations necessary to allow the robot to move are assigned. The starting configuration is 


"toc_dump.@Dec82.ral0’ — 





1100 1150 1200 1250 1300 1350 1400 1450 1500 1550 1600 


Figure A.29 - Yamabico’s Trajectory for Example Program 
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set to x=65.0, y=1200.0 and theta = 0.0 using the set_rob(&pstart) function. The six con- 
stant is set to the value of s by the function size_const (s) and the speed is set to 30.0 cm/sec 
by the command speed (15 . 0). The next two lines of code get the number of times the user de- 
sires the robot to drive around the racetrack. 

The robot first drives a straight line path (first_path), the automatically transitions to the 
next path. The robot drives around the racetrack controlled by the while loop. 

The function wait_until(X, GT, 1400.0) tells the robot to wait at level 0 until the 
robot’s odometry value of x exceeds 1400.0. Then the location trace function is turned off by the 
function loc_troff () and the robot is stopped using the halt () function. The wheel motors 
are turned off so the robot can be pushed by the command motor_on = NO. Then the robot’s 
location trace date file is transferred back to the host computer using the command loc_t rdum- 


p(“*loc_dump .8Dec92"), where “loc_dump . 8Dec92” is the using file. 


Definitions 


The following definitions are provided to assist the reader in understanding the terms 
used to explain the path tracking method of vehicle control. 

Closest Point (CP) - The point on the current path element that has the shortest Euclidean 
distance to the next sequential path element or to some point in the Cartesian plane. This applies 
to non-intersecting paths for determining the appropriate leaving point. 

Image - The projection of the robot’s current configuration onto the robot’s current path 
element. See Figure A.30. The image is represented as a configuration with the x and y components 
representing the closest point on the path element from the robot’s current odometry estimate. The 
values of 6 and x are the same as those of the path element at the image point. This information is 
expressed as a configuration. 

Immediate Function - Functions that are executed immediately when the command inter- 
preter reads them. This type of command is not held in a buffer for subsequent execution, instead, 
the affected parameters are changed immediately. 

Instruction Buffer - A first-in-first-out (FIFO) queue for temporary storage of pending ro- 


bot sequential commands. MML sequential instructions are interpreted and executed using a sim- 


September 2, 1993 Page 255 














Yamabico User’s Manual Naval Postgraduate School 





Figure A.30 Path Tracking Control 


ple producer and consumer paradigm. A producer task places the user instructions in the instruction 
buffer. A consumer task removes these instructions and executes them sequentially in the order 
they are removed from the instruction buffer. 

Intersection Point - The point or points of intersection in the Cartesian plane between two 
successive path elements. This is used as a first approximation of the transition point. When two 
path elements intersect at more than one point, the intersection points are involved; they are labeled 
upstream and downstream based on the intended direction of robot motion. 

Leaving Distance - The distance along the current path element from the leaving point to 
the intersection point. 

Leaving Point - (same as transition point) - The image on the current path element closest 
to the intersection point that does not result in oscillation in the transition to the next path. At this 
point the vehicle switches from tracking the current path to tracking the next path. 

Mode - The direction of vehicle motion on a circular path element. Counterclockwise mo- 


tion is a positive mode and clockwise is a negative mode. 
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directed 
parabolic path 


(Xp Yo 9 9) 
—_— 





Figure A.31 The Parabola Specification 


Parabola - A five element data structure used to describe a parabolic path element. The 
five elements are Xp Ip Xap Yop and 8 gr The parabola’s focus is represented by the point Xp yp) and 


the directrix of the parabola is the configuration (pp @, 0). Figure A.31 is an illustration of a 


parabolic path element specified by a focus and a directrix. 

Sequential Function - Sequential functions are robot control functions that are executed 
in the order received. Each sequential command must run to completion before the next one can 
start. Sequential functions are placed into the instruction buffer to await execution. Each sequential 
function awaits the logical completion of the previous sequential function. 

Transition Point - (Same as leaving point) - The image on the current path element closest to 
the intersection point that does not result in oscillation in the transition to the next path. At this 
point the vehicle switches from tracking the current path to the next path. 

Configuration - the physical location and orientation of a robot represented by p = (x, y, theta, 
kappa) where (x, y) is a point and the orientation theta is taken clockwise from the x-axis and kappa 


is the robot’s path curvature. 


September 2, 1993 Page 257 





Yamabico User’s Manual Naval Postgraduate School 


Navigator - an intermediate level on the Intelligence Module which receives the milestones of 
the future path from the Planner, and performs a more thorough search within the preferable stripe 
of this level, the results of this search are done at a higher resolution, and they are submitted to the 
Pilot as a guideline for the lowest level of IM. 


Pilot - the lowest level of the Intelligence Module which performs the synthesis of actual mo- 
tion trajectory. In other words the Pilot translates the output from the Navigator into the trajectory 


of the mechanical motion in accordance with the accepted set of elementary maneuvers. 


Pianner - the upper level of the Intelligence Module which performs the search for an opti- 
mum path at a lowest resolution. Planner determines the preferable stripe determined by the mile- 


stones, which is submitted to the Navigator for the subsequent stage of the planning. 


Sonar Fix - A determination of the robot’s current location or previous location using input 


from the sonar system and the world model 


Sonar Model - An abstract geometical model of a known world. This model is implemented 
in software and basically simulates the sonar returns that would be obtained by a robot at a given 
posture. The robot may utilize actual sonar returns, its dead reckoned posture and output from the 
sonar model, to fix its position in a given world. Additionally, this allows the robot to discriminate 


between known and unknown obstacles. 


Sonar Model 
The sonar model consists of line segments lineO through line11 representing the twelve Yam- 


abico sonars. Each line segment has a length of 4 meters, which is the ideal maximum range of the 
Yamabico sonar sensors. 

The world is modeled as a number of line segments. A function, segment_crossing_test(lineA, 
lineB) returns true if the line segments lineA and lineB cross at any point. The twelve Yamabico 
sonars are modeled mathematically as 4 meter long line segments. Each of the twelve sonar line 


segments is tested for crossing with the world line segments. If a sonar line segments crosses a 
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world line segment, the intersection r of the two line segments is determined. The distance from r 


to the robot’s current position is returned as the expected sonar range to the known obstacle. 
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APPENDIX B. LOCOMOTION SOURCE CODE 


[BEREEERESEREREEEESERESEELESEEESESE SESE SESESESESESESESESESES SEES ESSE ELSES 


FILENAME: control.c 

PURPOSE: path tracking functions for MML 
CONTAINS: 

control() 

pwm_lookup(vel) 

AUTHOR: Dave MacPherson 

DATE: | Feb 93 

COMMENTS: Needs more work. 


WHA AHHH HH HEHE EERE EEE EERE EE EEE EEEEEREREREREEESEREEESEEEREREREEES | 


#include “mml.h” 


[ARERR AAA REAR R AREER EEEEEEE EEE EERES EERE EERE EREREEEEEEEREEESEEES 


FUNCTION: control 

PARAMETERS: none 

PURPOSE: Reads robot encoders to update odometry 
every 10 msec (INTVL) and then sends 

commands to the motors that drive the 

wheels. 

perth pwm commands to drive the left and right drive 
wheels. 

CALLED BY: motor (assembly language code) 
CALLS: 

COMMENTS: 26 April 93 - Dave MacPherson 


ee Me He He He He He He Ae He HH HH AC HH HH A A RR HM HH RH HHA HAA HH ERA HEHE EEE EEE / 
control() 


register int lpwm, rpwm, bufpwm; 
register double v_l, v_r, cur_vl, cur_vr; 
register double dist_inc; 

register double dtheta; 

register double cur_v, cur_w; 

double delta_theta, delta_dist1; 


double pwm_lookup(); 

void store_loc_trace_data(); 

void next(); 

double read_left_wheel_encoder(); 
double read_right_wheel_encoder(); 
void get_velocity(); 


#ifndef SIM 


/* calculate the required linear and rotational velocities */ 
get_velocity(&uv, &uw, vehicle); 


261 








/* get encoder information returns how far the left and right 
wheels have moved forward in one step */ 


if (status != RMOVE) 

tread = TREAD;/* Narrower trend width for forward motion */ 
else 

tread = TREAD R; 

/* If robot is in the rotate mode use wider trend width */ 
tread2 = 0.5 * tread; 

dist_inc = (read_right_wheel_encoder() 

+ read_left_wheel_encoder()) / 2.0; 

Ss += dist_inc; 

Gtheta = (read_: right_wheel_encoder() - read_left_wheel_encoder()) / tread; 
cur_v = dist_inc / INTVL; 

cur_w = dtheta / INTVL; 

cur_vl = cur_v - tread2 * cur_w; 

cur_vr = cur_v + tread2 * cur_w; 

/* get left and right wheel velocity */ 





/* update current configuration */ 
next(&vehicle, dist_inc, dtheta); 
cur_x = vehicle.x; 
cur_y = vehicle.y; 
cur_t = vehicle.t; 
vehicle.k = kappa; 


/* trace loc */ 
if (Itrace_f != 0) 
store_loc_trace_data(vehicle.t, vehicle.k, uv, uw); 
/* if the vehicle’s motors are not on the return */ 


if (!motor_on) 
retum; 


#endif 


if (setting_configuration)/* for set_c function temporal exec */ 
{ 


setting_configuration = NO; 
vehicle.x = set_P.x; 
vehicle.y = set_P.y; 
vehicle.t = norm(set_P.t); 


/* calculate the required linear and rotational velocities */ 
/* get_velocity(&uv, &uw); */ 





if (emg_stp != 0) 
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uv = uw = 0.0; 





/* compute commanded left and right wheel velocities */ 
v_l = uv - tread2 * uw; 
v_r=uv + tread2 * uw; 


#ifdef SIM 
* 


* For the simulator compute vehicle’s configuration based on left 
* and right commanded wheel speed this is required in lieu of real 
* odometry 

*/ 


delta_theta = uw * INTVL; 

delta_dist] = uv * INTVL; 

vehicle.x += (cos(vehicle.t + delta_theta / 2.0) 
* delta_distl); 

vehicle.y += (sin(vehicle.t + delta_theta / 2.0) 
* delta_dist1); 

vehicle.t = vehicle.t + delta_theta; 

vehicle.k = kappa; 

#endif 


/* adjust pwm’s based upon the difference between the 
calculated wheel velocity and the odometry wheel 
velocity */ 

/* left wheel */ 

Ipwm = pwm_lookup(v_l) + kpw_b * (v_] - cur_vl); 


/* right wheel */ 
rpwm = pwm_lookup(v_r) + kpw_b * (v_r - cur_vr); 


#ifndef SIM 
/* set up motor control word (mcw) and threshold pwm values */ 
if (mv_direction < 0.0) 


bufpwm = Ipwm; 
Ipwm = -rpwm; 
rpwm = -bufpwm; 


mew = (mew & Oxf0f0) | (dpwm) > 0 ? 1 : 2) | ((pwm) > 0 ? 0x0100 : 0x0200); 
if (Ipwm > 127) 

Ipwm = 127; 

else if (Ipwm < -127) 

Ipwm = -127; 

if (Tpwm > 127) 

rpwm = 127; 

else if (r(pwm < -127) 

rpwm = -127; 








#endif 


} 


[EREREEREREREEEREEEEEEEEEEREREEEEEEEEESEEEREEEEEEERESERESEEESESEEEEERSEES 








returm (lpwm << 161 rpwm & Oxff); 


/* end control */ 


FUNCTION: get_velocity() 
PARAMETERS: uv, uw 


PURPOSE: Determines the robot velocity and rotational 


velocity based upon vel_c and kappa. 
RETURNS: *uv, *uw 
CALLED BY: control() 


CALLS: update_vel(), update_kappa(), transition_point_testQ) 


COMMENTS: 23 Apr 93 - Dave MacPherson 
TASK: Level 4 interrupt 


BREAKER EA EERE EEE EER KEE EEE EEE EEE A KER EE EEE EKER EE EEE EERE EERE EEE KEELE EE / 


void get_velocity(uv, uw) 
double *uv, *uw; 


{ 


/* 


*/ 


PATH_ELEMENT path; 
switch (status) 


case SSTOP: 
length_s = 0.0; 
vel_c = 0.0; 

if (wait_cnt == 0) 


(*uv) = vel_c; 
(*uw) = 0.0; 
read_inst(); 

} else 
wait_cnt--; 
break; 


case SPWAY: 

(* uv) = vel_c = update_vel(); 
if (sonar(4) == -1); 

kappa = 0.0; 

else if (sonar(4) > 50.0) 
kappa = -0.01; 

else if (sonar(4) <= 50.0) 
kappa = 0.01; 

(* uw) = kappa * vel_c; 
break; 


case SLINE: 
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(*uv) = vel_c = update_vel();/* commanded velocity */ 
kappa = update_kappa(); 
(*uw) = kappa * vel_c;/* commanded omega */ 


if (get_inst != put_inst) 
if (transition_point_test(current_image, get_inst->tp)) 


--no_o_paths; 
current_robot_path.pc = get_inst->c; 
current_robot_path.type = get_inst->class; 


read_inst(); 
}/* end if */ 


} 
if (skip_flag_control) 


current_robot_path.pc = get_inst->c; 
read_inst(); 

skip_flag_control = 0; 

} 

break; 


case SBLINE: 
(*uv) = vel_c = update_vel();/* commanded velocity */ 
kappa = update_kappa(); 
i = kappa * vel_c;/* commanded omega */ 
Kk 
* if (no_o_paths > 1) if 
* (transition_point_test(current_image, path, 
* get_inst->tp)) { --no_o_paths; 
* current_robot_path.pc = get_inst->c; read_inst(); 
* 


* 


if (vel_c < 0.5 && EU_DIS(current_image.x, current_image.y, 
current_robot_path.pc.x, current_robot_path.pc.y) < 1.0) 


{ 
status = SSTOP; 
read_inst(); 


} 

if (skip_flag_control) 

{ 

current_robot_path.pc = get_inst->c; 
read_inst(); 

skip_flag_control = 0; 

break; 

case SCONFIG: 


(*uv) = vel_c = update_vel();/* commanded velocity */ 
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oe 
7 if (first_time) { first_time = FALSE; 


* } else call update_cubic_image to advance image 
*/ 


current_image = update_cubic_image(vehicle, current_robot_path); 


i* 
* Now update the global kappa that is used to 
* control the robot’s actual motion 
*/ 


kappa = update_cubic_kappa(vehicle, current_image); 
(*uw) = kappa * vel_c;/* commanded omega */ 


/* 
* There are many tests to see if the end of the 
* spiral has been reached, easiest is to compare 
* image_s to precomputed length of spiral, stored in 
* pp.x0 
*/ 


: (image_s > current_robot_path.pp.x0) 


read_inst(); 
}/* end if */ 
break; 


case SPARABOLA: 

break; 

case RMOVE: 

(*uv) = 0.0; 

(*uw) = rvel_c = get_rotational_vel(); 
break; 

case SERROR: 

vel_c = update_vel(); 

if (vel_c <= VEL1) 


vel_c = 0.0; 
motor_on = ON; 
break; 

default: 

(*uv) = 0.0; 
(*uw) = 0.0; 
break; 


}/* end switch */ 
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} /* end get_velocity() */ 


[BERREREEEREEEEEEEEEEESERESESESEEEEEEESEESESEEESES SEES EES EEESESE SESS EES 


FUNCTION: read_left_wheel_encoder 
PARAMETERS: none 

PURPOSE: Determines the distance moved by the left 
wheel by reading the optical encoder. Filters the 

data using a recursive digital filter. 

RETURNS: dist_l ( The distance moved by the right wheel 
in the current vehicle control cycle ). 

CALLED BY: control() 

CALLS: none 

COMMENTS: 20 Apr 93 - Dave MacPherson 

TASK: Level 4 interrupt 


MAAK KHER EES ERREEKERE ERE EEEESEREREREREEEEESEREREREEESEREEEREEEEES | 


double read_left_wheel_encoder() 
{ 


double a = 0.7;/* filter constant */ 
double dist_1; 


if (mv_direction > 0.0) 

dist_l1 = mv_direction * dlenc * ENC2DIST; 
else 

dist_] = mv_direction * drenc * ENC2DIST; 


/* Recursive Digital Filter */ 
dist_] =a * dist_] + (1 - a) * last_dist_]; 
last_dist_l = dist_l; 
return dist_]; 
} /* end read_left_wheel_encoder */ 


[RAR RAHA RARE HEHEHE AER EER H ARERR BARE ERE ERE EERE ERR EEE HE EE ERE 


FUNCTION: read_right_wheel_encoder 
PARAMETERS: none 

PURPOSE: Determines the distance moved by the right 
wheel by reading the optical encoder. Filters the 

data using a recursive digital filter. 

RETURNS: dist_r ( The distance moved by the right wheel 
in the current vehicle control cycle ). 

CALLED BY: control() 

CALLS: none 

COMMENTS: 20 Apr 93 - Dave MacPherson 

TASK: Level 4 interru 


pt 
HRA AAA HH HA EEK AK ARE EEAK KERR REE EERE KEKE EEE EEA EREREE EERE EEE EREK ERE | 
double read_right_wheel_encoderQ) 


double a = 0.7;/* filter constant */ 
double dist_r; 


if (mv_direction > 0.0) 
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dist_r = mv_direction * drenc * ENC2DIST; 
else 
dist_r = mv_direction * dienc * ENC2DIST; 


/* Recursive Digital Filter */ 


dist_r = a * dist_r + (1 - a) * last_dist_r; 
last_dist_r = dist_r, 
return dist_r; 

} /* read_right_wheel_encoder */ 


[ERREREREEEEREEEREREREEEEEEEEREREEREREEEEEEERESESESESEEEESEERESEEEEERERE 


FUNCTION: next 

PARAMETERS: q, delta_s, delta_theta 

PURPOSE: Updates the robot’s current configuration based upon 
the input values of delta_s and delta_theta. 

RETURNS: *q (a pointer to a configuration ). 

CALLED BY: control() 

CALLS: none 

COMMENTS: 19 Apr 93 - Dave MacPherson 

TASK: Level 4 interrupt 

Me MM A He HH HAH HH RR AHA ARE AEH ERE EEE EEE SEES EERE EERE REEEEE EEE EE EE EE / 
void next(q, delta_s, delta_theta) 

CONFIGURATION *q; 

double delta_s, delta_theta; 


double sinc; 
double dtheta2 = delta_theta / 2.0; 


sinc = delta_s; 
if (delta_theta) 
sinc *= sin(dtheta2) / dtheta2; 


/* Update The vehicle’s odometry estimate */ 
q->x += sinc * cos(q->t + dtheta2); 
q->y += sinc * sin(q->t + dtheta2); 
q->t = q->t + delta_theta; 
} /* end next */ 


[RREREREREREERE REE EREEAAEEEEEEEREE ERE EER EEE EE EEKEREEE EEE ERE LE EE EREAERERE 


FUNCTION: pwm_lookup 

PARAMETERS: vel (wheel velocity) 

PURPOSE: Determines the estimated pwm ratio given 

the desired wheel velocity as an input. 

RETURNS: pwm value based upon empirically determined velocity 
vs pwm ratio curve. 

CALLED BY: control() 

CALLS: none 

COMMENTS: 12 Jan 93 - Dave MacPherson 
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TASK: Level 4 interrupt 
SESEREEEESESREESESEEERERESESESESESSESESESSESESEEEESESESESESESELESESE SES / 
double pwm_lookup(vel) 

double vel; 


double v = vel; 
double pwm_value; 


v = fabs(vel); 

if (v >= 0.0 && v < 20.0) 

pwm_value = (0.5 * v + 13.0); 

else if (v >= 20.0 && v < 30.0) 
pwm_value = (1.256 * (v - 20.0) + 23.0); 
else if (v >= 30.0 && v < 40.0) 
pwm_value = (2.413 * (v - 30.0) + 35.56); 
else if (v >= 40.0 && v < 50.0) 
pwm_value = (1.651 * (v - 40.0) + 59.69); 
else if (v >= 50.0 && v < 60.0) 
pwm_value = (2.54 * (v - 50.0) + 76.2); 
else if (v >= 60.0 && v < 65.0) 
pwm_value = (5.08 * (v - 60.0) + 101.6); 
else 

r_printf(‘‘Error in pwm lookup function”); 


if (vel > 0.0) 
return pwm_value; 
else if (vel < 0.0) 
return -pwm_value; 
else 
return 0.0; 

} /* end pwm_lookup */ 


[BERR RRR ERE AREER ERE KARE EEE ERK EE ERE EEE EEE EEE EEE EE HEE EERE EEEEEE 


FUNCTION: store_loc_trace_data() 
PARAMETERS: arg], arg2, arg3, arg4 
PURPOSE: Records location trace data if enabled. 
RETURNS: void 

CALLED BY: control() 

CALLS: none 

COMMENTS: 22 Jan 93 - Dave MacPherson 
SERRE REEERER EERE EAREREREREEEKEREREE EEE EEEREREEEEEREEREEEEREREEREEED YS & / 
void store_loc_trace_data(arg1, arg2, arg3, arg4) 
double arg1, arg2, arg3, arg4; 

{ 


if (lop_tr == 0) 

{ 

if (scale_tr == 1) 

{ 

*(indxloc++) = time_tr; 


} else 
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Sindxioc#+) = vehicle.x; 
if ((pattern_tr & 1) != 0) 
Sindxloc+#) = vehicle.x; 
} ((pattern_tr & 2) != 0) 
Lasieheia) = vehicle. y; 
if ((pattern_tr & 4) != 0) 
*(indxloc++) = arg]; 

if ((pattern_tr & 8) != 0) 
*(indxloc++) = arg2; 

: ((pattern_tr & 16) != 0) 


*(indxloc++) = arg3; 


} 
if ((pattern_tr & 32) != 0) 
Co = arg4; 


trace_cnt++; 
lop_tr = smpl_tr; 
} 


lop_tr--; 
time_tr += 0.01; 
} /* end store_loc_trace_data */ 


/* control.c */ 
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SEKKEKREKEERESSECSEEEKKKECKSEKESSESEKESKEKSEESEKESESESEEESSESSESESKESESESESESEE 


immediate.c 
Rev 0 May 15, 1993 by Dave MacPherson 


REKKKEKKAEKSEKKESCHEAKESKESKSERKSESSSESSESSKKSESKESSEKKEKSESKSESKHSSSEKKEKKEKEESKES 


*/ 
/##44%44444% INCLUDED FUNCTIONS **#####48944/ 


[eeeeeeee IMMEDIATE FUNCTIONS REKSEKEKEE 
stop0() 
get_rob(p) 
get_rob0(p) 
speed0(s); 
r_speedQ(s); 
accO0(a); 
r_acc0(a); 
skip(); 
get_line(); 
haltQ) 
resume() 
sync() 
path_lengthQ); 
flush(); 


MMH HAH MH HHA HHH AH EH REARS E ERASE AAA EEE EEE KES EK EAE EF / 


#include “mml.h” 
#include “spatial.h” 


/* 
94 2 he ae a he He ee he 2 He ee he ae he ae he a he ee he He 2 he he a 2 he a ae he ae ae 2h he ee ee 2 he he oe ake a 2 he oe 2 ake he ae he a ae i eK Kk 


IMMEDIATE FUNCTIONS 


Me he He 2he 2 he ae abe ae he he ae he ae 2h fe 2h ae he fe 2h ae 2h he 2h he he he he He 2h He ake ae 2h He 2h he she a 2h He ake he 2k ae he He 2h he he he 2k he he fe he ae he ae he a he 2 he ok i a 


ad | 


[EERE EEEEREER EELS ER EEREREEREEEE ER ERE EEE EEE EEEREEEEEEKEREEREREERERERE 
FUNCTION: path_length (immediate) 

PARAMETERS: none 

PURPOSE: get the total path length traveled by the abot 

RETURNS: double 

CALLED BY: user 

CALLS: none 

COMMENTS: 7 Jan 93 - Dave MacPherson 

TASK: Level 0 


ERRREEAEEEREREREEEEREEEES EEE REEAEEREREEE EERE RERESEEEEREEE EEE ERELERE SES / 


double path_length0) 
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double arg_; 
{ 


racc = arg_; /* set the robot’s translational acceleration */ 
} * end raccO */ 


[BEREERESEESSEERESEEESESESEEEESESEESEEESESESESESEEESESEEEEEEESESESES ES ES 
NAME : get_lineO 

ARGUMENTS : none 

FUNCTION : return a pointer to the path element the robot 

is currently tracking. 
SREEREREEESSERESESEREEEESEEREEELERESEEESESEEEEEESESERESESEESERESESESESS / 


CONFIGURATION get_line0Q 


{ 
return get_inst->c; 
} * end get_lineO */ 


[BRRRRREREEAEEEERREEREAEEEEREEE EEE ER ERE LEER EEE EEE EEREEEREREREREREEEREREE 


NAME : stop0 
ARGUMENTS : none 
FUNCTION : stops the robot and flushes the instruction buffer. 


2H oe oe eh te he hee ee He He a he he eH a a a RE HAAR E REE EEEREREEEKEREREEERALEREE EES / 


void stop00Q 
{ 
vel_g = 0.0; /* set robot goal velocity to zero */ 


/* flush instruction buffer here */ 

head_inst = put_inst = get_inst = &inst_buf[0]; 
inst_cnt = 0; 

tail_inst = &inst_buf[INST_MAX-1]; 

head_len = put_len = get_len = &length_buf[0}; 
tail_len = &length_buf[INST_MAX-1]; 


seq_status = SSTOP; 
r_printf(“‘ \12 Entered the stop0() function.”); 
} /* end stop0 */ 


[ERR RA AHH ARENA HAKKAR HHH EAR REAR EHH EK EEE EEE REK EE 


FUNCTION: flush 
PARAMETERS: none 
PURPOSE: discards all buffer commands after current_robot_path 


RETURNS: void 

CALLED BY: user 

CALLS: imaskoff()? 

COMMENTS: 11 Jun 93 -- Bob Fish 
TASK: Level 0 


BREE EREKEEEREGEEEARAEREEELEEREEEEEEERES EEE EE EEE EEEEEEREREEES EEL EERE SE / 
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void flush() 
{ 
int i; 


/* First step is to reset put_inst to be the same as get_inst. 

That way, the next motion command loaded on the buffer will be 
stored after the path currently being tracked. 

Second step is to reset global last_robot_path_element to be the 
current_robot_path. That way, when a new motion command is issued, 
the transition point calculations will be between the current path and 
the new path. no_o_paths is set to 1, because now there is only one 
path, the current one. */ 


i = imaskoff(); 


put_inst = get_inst; 
no_o_paths = 1; 
last_robot_path_element = current_robot_path; 


r_printf (“\nLRP.type=> “); 
r_printfi(last_robot _path_ element.type); 
r_printf (““nLRP.x=> “); 

r_printfr(last_; robot __path_element.pc.x, 2); 
r_printf (“ y=> “); 
r_printfr(last_robot_path_element.pc.y, 2); 
r_printf (“ th=> ye 
r_printfr(last_robot_path_element.pc.t, 2); 


imaskon(i); 


return; 
}/* end flush */ 


size_const0(size) 
double size; 


double kk; 


DIST_CONSTANT = size; 

kk = 1.0 / DIST_CONSTANT; 
aa = 3.0 * kk; 

bb = aa * kk; 

cc = bb * kk /3.0; 

} /* end size_const0 */ 


/* 
Ihe he he he se he ae he ae he be abe fe he af fe fe fe he ake he ae abe ake he afc she she he 2c 2c eae ae ae ae ae ae he he ee eae fe he he ae ae ae ae fe ae ae ae he ae ae ae he Se Se ae ae eee eae oe 


NAME : get_rob0 
ARGUMENTS : A pointer to a CONFIGURATION 
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FUNCTION : return current odometry estimate from the controller. 
8EEESESRSEERSESEESSCEEKSESEEESESESESESESKESESEESESEEESEEKECKEESKESESESEKESESEEESEEE 
ei 

CONFIGURATION *get_rob0(p) 

CONFIGURATION *p; 

{ 


inti; 


i = imaskoffQ); 
p->x = vehicle.x; 
p->y = vehicle.y; 
p->t = vehicle.t; 
imaskon(i); 
return (p); 

} /* end get_rob0 */ 


/* 

ake he 2h ec ie ae He he he 2h he ae he ae He he He 2h 2h he ae he he he 2s he he he cc ae ae he ae he ee he 2k he ee a a oe i he ee KK KK KK KKK KKK 
NAME: set_rob 

FUNCTION : Set postures of cur. 


Me ee he He He He he He he He HH HHH HAHAHAHA KAKA HA AAKAKAAKAKAAAEKSEKHERAKAKKHEKKKKKKKKAKKKAKEKE 
3 
void set_rob0(p) 
CONFIGURATION *p; 
{ 
int i; 
i = imaskoffQ; 
set_P.x = p->x; 
set_P.y = p->y; 
set_P.t = p->t; 
setting_configuration = YES; 
imaskon(i); 
} * end set_rob */ 


double halt_speed = NEGATIVE_SPEED; 


bd 
He Hee He He HH He He He He He AH KK KK KK HEH EEE RKEKKEKKKKAKKAKKEREKAEREREKEKEKEREK 


NAME: halt 

This function brings the robot to a smooth stop and places it 
in a dormant state. The robot will not respond to any ¢ ther 
commands until resume() is called. All motion parameters are 


restored by resume() to their values prior to the call to halt(). 
Be He ae He He He He He He He Me Me He Me He He He Me He ake fe He He ae He He He He He He she He he “fe He He He He He Me He ee He He he Me He he He He He He ae ae He He ee he ae he 3 Ae ake 2h a eK 


“/ 

void halt() 
if (halt_speed > 0.0) 
return; 


halt_speed = vel_g; 








vel_g = 0.0; 
} * end halt */ 


i* 
REAKKKKAKEKKKAKKSEKSEKKKKSSKSESKRAKAKSEKASBHAAKAKHSEKAKESEEKEEKEKESEKEKEKEKELKSE 
NAME: resume 

This function resets the robot’s motion which was suspended 

by the call to halt( to the last user values. 


MMA BKAKEAKHKAKEKAAKKAKKKAAKAERKEKKEKEKKKEKKEEKSESEKESEEKESEKEKESEEKESESREEREERE 


*/ 
resume() 

if (halt_speed < 0.0) 

return; 

vel_g = halt_speed; 

halt_speed = NEGATIVE_SPEED; 
} /* resume() */ 


/* end immediate.c */ 
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BEKKEKEKESELSESESSESESESESESESSSESESKSEKESEEEESESEKSEKSESSEKESESESSESSEKSEKETESLE 


sequential.c 

Rev 0 May 15, 1993 by Dave MacPherson 
RAKKKKKKHSEKEKKEKKEEBSESSESEKSSRKESESEKSKSSESKSEESSESKSESSSESEEKSESESESSESSESEEKEKESRE 
‘a4 
[REREEREEEEE INCLUDED FUNCTIONS BEEEESESEEES / 


[He eERERE SEQUENTIAL FUNCTIONS BEEF KEKEKE 
speed(arg_) 
acc(arg_) 
rotate(thetasp) 
r_speed(arg_) 
r_acc(arg_) 
mark_motion() 
wait_motion() 
config(arg_) 
line(arg_) 
bline(arg_) 
fline(arg_) 
switch_dir() 
set_rob(pst) 
size_const(arg_) 


HR RRR HHH HHA EAH E HERRERA KE KHER A AEE EK AREER EEE EES EEE EE / 


#include “mml.h” 
#include “spatial.h” 


HHA A HAR HAA MH HH A AA ENE HAH HH HARA HH HHH HHA ACH HHH | 


void set_error(code) 
int code; 


{ 
PATH_ELEMENT path; 


path.type = ERROR; 
path.mode = code; 
set_inst(path); 
} /* end set_error() */ 


[RRR RN A A HH AT HH HH AH HH HANH AAA AA BH HAHAHA RHEE AAA AE HAE KH 


FUNCTION: size_const sequential 

PARAMETERS: size 

PURPOSE: Sets the parameter DIST_CONSTANT in a sequential 
fashion. This determines how sharply Yamabico turns. 
RETURNS: void 

CALLED BY: user 

CALLS: set_instQ); 
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COMMENTS: 7 Jan 93 - Dave MacPherson 

**#** DOES NOT LOAD INST BUFFER AS ADVERTIZED ****#*#### 
TASK: Level 0 
EREEEAELEEEEEEREREREREEEESEEEEEEEEEEERESESEEEESESESESESEESEEEREES SEES / 
size_const(size) 
double size; 


PATH_ELEMENT path; 
double kk; 


DIST_CONSTANT = size; 
kk = 1.0 / DIST_CONSTANT; 


path.type = SIZE; 
path.pc.x = size; 


set_inst(path); 
} /* end size_const() */ 


[RARER AHR EEAA RAH EEE EEE BREET EERE ERASE EAH EE EEE EK EEEEEE EE 
FUNCTION: speed (sequential) 

PARAMETERS: arg_ 

PURPOSE: to set robot’s speed 

RETURNS: void 

CALLED BY: user 

CALLS: set_inst 

COMMENTS: 7 Jan 93 - Dave MacPherson 

TASK: Level 0 

HAA HHA AHA AA RRA BRAK ERIE HARE EEK HH HAAR BREE EEA ERAN H EERE KEE A HERE EK / 
speed(arg_) 

double arg_; 


{ 
PATH_ELEMENT path; 


path.type = SPEED; 
path.pc.x = arg_; 


set_inst(path); 
} /* end speed() */ 


[FERRER AAR R REAR ERK HH HR HERA AHH EREEE KERR EEA RARER EER ERE EE 
FUNCTION: acc (sequential) 

PARAMETERS: arg_ 

PURPOSE: to set robot’s acceleration for speed changes 

and stopping. 

RETURNS: void 

CALLED BY: user 














CALLS: set_inst 

COMMENTS: 7 Jan 93 - Dave MacPherson 

TASK: Level 0 

BEREREREEREEREESEEEEEEEREEEEEREEEREEEERE EEE EEEERESEEEEEREREEEEESERERESS / 
void acc(arg_) 

double arg_; 


PATH_ELEMENT path; 


path.type = ACC; 
path.pc.x = arg_; 


set_inst(path); 
} * end acc() */ 


[RARER RARE AAR R EEE EEE RRR ER EEE EREEE EERE EERE EE EEREREEEERE EEE EEERERERS 


FUNCTION: rotate (sequential) 
PARAMETERS: arg_ 

PURPOSE: Rotate the robot by thetasp radians. 
Positive is counterclockwise and negative is 
clockwise. 

RETURNS: void 

CALLED BY: user 

CALLS: set_inst 

COMMENTS: 24 March 93 - Dave MacPherson 
TASK: Level 0 
Jdidodddaddaiidiicee aiid i ao i Ga TOE a IO AIO AI I TTR ATR ATR RT / 
void rotate(thetasp) 

aa thetasp; 


PATH_ELEMENT path; 


if (seq_status != SSTOP && seq_status != SBLINE) 
/* robot must be stopped to shift to rotate */ 


{ 
set_error(ECODE2); 
retum; 


} 
/*thetasp = d2r(thetasp); 04/15/92 */ 
if (fabs(thetasp) < 0.0001)return; 
path.type = ROTATE; 
path.pc.x = thetasp; 


set_inst(path); 


last_robot_path_element.pc.t += thetasp; 
last_robot_path_element.type = SET_ROB; 


nom_p->t += thetasp; 
seq_status = SSTOP; 








} * end rotate() */ 


[PRERREEREEEEEEESEEEEEEEEEESEEEEESESESESEESESESES ES ERES ESE EES ESSESEES ERS 


FUNCTION: rspeed (sequential) 

PARAMETERS: arg_ 

PURPOSE: to set robot’s angular speed to be used 

when the robot performs a stationary rotation. The parameter 
for rotational speed is in radians/second. 

RETURNS: void 

CALLED BY: user 

CALLS: set_inst 

COMMENTS: 8 Jan 93 - Dave MacPherson 

TASK: Level 0 

BEEK REREARERB ERASER EEK EREREE EKER EEREEEEEREREREEEEEREEEEEEESE EEE / 
void r_speed(arg_) 

double arg_; 

{ 


PATH_ELEMENT path; 


path.type = RSPEED; 
path.pc.x = arg_; 


set_inst(path); 
} /* end r_speed() */ 


[RRA ARERR RARE HEAR EERE REALE EE EEEER EEE EEEEEEE RS AEEEERERE EEE EEE EERE 


FUNCTION: racc (sequential) 

PARAMETERS: arg_ 

PURPOSE: to set robot’s angular acceleration to be used 
wheel the robot performs a stationary rotation. 
RETURNS: void 

CALLED BY: user 

CALLS: set_inst 

COMMENTS: 8 Jan 93 - Dave MacPherson 

TASK: Level 0 

He ee ee He eee He A AH HM A HH A HH HR HHH HA HHH HAA EEE EAE EEK EE HAE | 
void r_acc(arg_) 

double arg_; 


PATH_ELEMENT path; 
path.type = RACC; 
path.pc.x = arg_; 


set_inst(path); 
} /* end r_acc() */ 
[FERRER E EERE REE RENEE EA EEE EREEE EEE EREEEEEEES / 


/* designate syncronization to yamabico.*/ 
/* Jan. 23 89*/ 
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[BEEEERESEEESESESESEERESESEEEEERESESEERESESESESES/ 
mark_motion() 


msyn_q = 1; 


[REREEAEEREREEEESESEESESERESERESE SEE EE EEESEEEERES / 
/* execute syncronization*/ 
/* Jan. 23 89*/ 


[BREREEREREEEERERESERESERESEEEEESELERESEEESESES ES / 
wait_motionQ 


wsyn_q = 1; 
while(wsyn_q!=0); 


[FREER RREEEREAEEREREEEERERERERERER ERE EEE EERE EEEEEEEEEEEEE EERE EERE EEE EEE 


FUNCTION: skip() 

PARAMETERS: none 

PURPOSE: Causes the robot to skip the next sequential motion 
command. 

RETURNS: void 

CALLED BY: user() 

CALLS: none 

GLOBALS: skip_flag 

COMMENTS: 26 Feb 93 - Dave MacPherson 


HMM HH HHH HMM HH A A HE EAE RARE AAR ERE KKK E EEE E EERE ERA A KER EEE REE EKER / 


void skipQ) 

{ 

skip_flag = TRUE; 
} 


[BRERREAAA RARE RAAB ER EEE EEE REA KEKE EEEE EE HEE EEA E EEE RE AAR R EERE EE 


FUNCTION : config(arg_) 
PARAMETERS: configuration arg_ 
PURPOSE: Implements users command to move to a 
specified configuration using 
one or two cubic spirals. 
RETURNS: void 
CALLED BY: user.c 
CALLS: solve (located in file cubic.c) 
GLOBALS: seq_status - set; 
COMMENTS: 8 Feb 93 -- Bob Fish 
TASK: level 0, foreground job. 
HEEEAEEKAEEREREEK EEE EE EEREEES HE EREEEEEEREREE EEE EREEE LER ERESERERERERS / 
config(arg) 
CONFIGURATION “arg; 
{ 
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CONFIGURATION end_spiral; 
CONFIGURATION start_spiral; 
int res; /* flag to indicate success or failure of function solve */ | 





if (seq_status == SLINE) 

set_error(ECODE3); | 

r_printf(‘\n\nLINE to CONFIG Configuration Combination not Allowed.\n\n”); | 

/* exit(0); */ 

} 

else if (seq_status == SPARABOLA) 

{ 

set_error(ECODE3),; 

r_printf(‘“n\nPARABOLA to CONFIG Configuration Combination not Al- 
lowed.\n\n’”’); 

else if (seq_status == SFLINE) 


set_error(ECODE3); 
r_printf(‘\n\nFLINE to CONFIG Configuration Combination not Allowed.\n\n”); 
} 





else 


{ 


end_spiral = (*arg); 


/* Values of start_spiral are obtained from the last motion instruction 
in the buffer: 

NOTE NOTE NOTE: this requires that the last motion instruction 

be a legal precedent for a cubic spiral */ 


start_spiral.x = last_robot_path_element.pc.x; 
Start_spiral.y = last_robot_path_element.pc.y; 
Start_spiral.t = last_robot_path_element.pc.t; 
start_spiral.k = 0.0; 


/* Call solve with start_spiral and end_spiral as beginning and end of 
the cubic spiral(s). */ 


res = solve(start_spiral, end_spiral); 


/* ‘res’ can be used to see if the cubic spiral was successful. res may not 
be useful for anything else that I can see, unless an error develops */ 


seq_status = SCONFIG; 
/* Update last_robot_path_element to latest path */ 


last_robot_path_element.pc = end_spiral; 
last_robot_path_element.type = SCONFIG; 
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return; 


} ** end config() */ 


/* 
SEKKEKSEKEKSSSESKSKSKKSSKEKKEKKKESEKSESKSEESSSKAKSSSESEKSESKESKKSKESEAKESEECESE 
NAME : line configuration 
ARGUMENTS : configuration of path the robot must follow 

CTION : to move robot to a path 
REKKRAKKEKKAEKKEKSEKKEKKKAKKSEKEKKKEKESLKKKKKKAKEKSERKEKKKESKESRKKKESKEKKEKESEES 
*/ 
line(arg_) 
CONFIGURATION *arg_; 


{ 
PATH_ELEMENT path: 


path.type = LINE; 
path.pc = (* arg_); 
if (no_o_paths == 0 II skip_flag) 


r_printf(‘\nFirst path, no transition point”); 
last_robot_path_element.pc = path.pc; 
last_robot_path_element.type = SLINE; 
ie aie =]; 


else 


path.tp = 
get_transition_point(last_robot_path_element, path); 


r_printf(‘NnTransition point to line\n x = “‘); 
r —printfr(path.tp.x0, 2); 

t_printf(“ y =“); 

r_printfr(path.tp.y0, 2); 

++no_o_paths; 

last_robot_path_element.pc = (*arg_); 
last_robot_path_element.type = SLINE; 


} 

set_inst(path); 

seq_status = SLINE; 
} * end line() */ 


/* 
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SREKKKECKSESRESSKASKSESSESRESESSHSEKSEASASEKESHSSLSSESESSERESESSSESSSSKEKSESKSEESE 


NAME : backward line configuration 

ARGUMENTS : configuration of path the robot must follow 

FUNCTION : to move robot to a path 
SRELKSKSKSEKKKESESEKSKESKSSKEEKKSESERERSSSKKESSEESKEKSESEESESESESESEKECESCECESESES 
*/ 

void bline(arg_) 

Siac *arg_; 


PATH_ELEMENT path; 


P 8); 
if (no_o_paths == == 0 Il skip_flag) 


last_robot_path_element.pc = path.pc; 
last_robot_path_element.type = SBLINE; 
no_o_paths = 1; 


else 

{ 

path.tp = 
get_transition_point(last_robot_path_element, path); 


r_printf(‘\nTransition point to bline\n x = “); 
eee epee tp. x0, 2); 

T_printf(“ 
r_printfr(path.tp.y0, 2) 


++no_o_paths; 
last_robot_path_element.pc = (*arg_); 
last_robot_path_element.type = SBLINE; 


} 
/* set the robot’s desired path to the value of arg_ */ 


set_inst(path); 
seq_status = SBLINE; 


} * end bline() */ 


[RRA RAA AAA AEA EEA REKERE EERE EEKEREREK EE EEE AEEE KEES EK EKEAAK KEE 


FUNCTION: fline (arg_) 

PARAMETERS: arg_ 

PURPOSE: To cause the robot to follow a straight line starting at a specific point. 
RETURNS: void 

CALLED BY: user0 

CALLS: config, line 

GLOBALS: none 

COMMENTS: 24 Feb 93 - Bob Fish. Since this is a compound command, printouts 
will indicate a cubic spiral followed by a line. 


BEARER EE ERE EERE EERE REL ERE REE EE ERE EER EEE EEEERE ERE EREERERREEE EERE RE / 
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fline(arg_) 
(oe *arg_; 


if (seq_status == SLINE) 


{ 
set_error(ECODE3); 
r_printf(‘“\n\nLINE to FLINE Configuration Combination not Allowed.\n\n”); 


} 
else if (seq_status == SPARABOLA) 


{ 

set _error(ECODE3); 

r_printf(‘“n\nRPARABOLA to FLINE Configuration Combination not Al- 
lowed.\n\n”’); 


} 
else if (seq_status == SFLINE) 


set_error(ECODE3); 
r_printf(‘\n\nFLINE to FLINE Configuration Combination not Allowed.\n\n”); 
} 


else 


/* This is implemented as a compound command. First call config to 
generate a cubic spiral path to the specified point configuration, 

then call line to track the line that goes through that point, 

with theta and kappa as specified. 

NOTE: This precludes some path reporting capability, since from this 
point on, the concept of fline is lost, 

it is replaced by config and line. */ 


config(arg_); 
line(arg_); 


} 


i* 
ee 2 he eae He ae He ae he ae he ae Me ae He ae he Me Me He He ae Me Me Me Me he he he he he ae afc 2h shee she ae he ae he ae he ae ake ake he Ae he ae he ae Me ae he He He He He He He He He He Hk He ak He 
NAME : parabola point directrix 
ARGUMENTS : configuration of path the robot must follow 
CTION : to move robot to a specified path 
Me Me ae ae Me ae ae ee ae 2c ae he ae he ae 2h ae he ae he he ake 2 ah 2h he 2h ae he abe eae Ae ake Me he He she ae he he ae ae she ae Me 2 he ae Me He ee ae He He He He He he He he He He He He He ke 
*/ 
parabola(focus, directrix) 
POINT *focus; 
CONFIGURATION ‘directrix; 


PATH_ELEMENT path; 
if (seq_status == SPARABOLA) 
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{ 

set_error(ECODE3); 

r_printf(“n\nLINE to PARABOLA Configuration Combination not Al- 
lowed.\n\n”); 


} 
else if (seq_status == SCONFIG) 


{ 

set_error(ECODE3); 

r_printf(‘“\n\nCONFIG to PARABOLA Configuration Combination not Al- 
lowed.\n\n’”); 

} 


else 


path.type = PARABOLA; 
path.pc = (* directrix); 
path.pp = (* focus); 


set_inst(path); 


/* set the robot’s desired path to the value of arg_ */ 
seq_status = SPARABOLA; 
} 


Vctinaaaicitinactinaietiiieaiaiciediatncineteiiaatatialaiadniiaiatindiediadntatindiatiaiatidiaintntietintntntiniintiatidiatntediatataiidiatadadetaiaiiiiel 


FUNCTION: move_hall_follower0 
PARAMETERS: arg_ (distance to walls) 
PURPOSE: Causes ike robot to follow a hallway by 
perform odometry corrections in the background. 
RETURNS: void 

CALLED BY: user() 

CALLS: set_instQ; 

GLOBALS: none 

COMMENTS: 8 May 93 - Dave MacPherson 
HHRMA KA AAH AKA AREER AHR AREA R AA KRHKR A K ERE EERE EERE EERE EERE AEE EEE / 
void move_hall_follower(arg_) 

ies arg_; 


PATH_ELEMENT path; 
r_printf(“Entered the move_hall_follower function”); 
set_inst(SPWAY, arg_); 


seq_status = SPWAY; 
} /* end move_hall_follower */ 


* 
Hehe he He he he He Ke ee He He HK A HK HAH AE KEKE KAKKKAAKKKAKKKAKKAKKAEKKAKAKKAKKAKCEKHEKKE 
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NAME : switch_dir 
ARGUMENTS : none 
FUNCTION : to reverse the heading direction of the robot 


PRAHA AAKKAKRKERKAAESKAKKAKSKSEKSKERKESSKEKRSERESSSESESSKEKEKEEEKECKESKEESEESEE 


*/ 
ec 


PATH_ELEMENT path; 
if (seq_status != SSTOP) 


{ 
set_error(ECODE2); 
return; 


} 
path.type = SWITCH; 
set_inst(path); 
nom_p->t = norm(nom_p->t + PI); 
} * end switch_dirQ */ 


* 
be sa erat cx waustuseesccusascuewssusvadavescaesscssteusitasicaleciviese 
NAME: syncQ) 
ARGUMENTS : none 
FUNCTION : syncronizing to locomotion data update 
sync_loc flag is modified in _ih.loc routine 
Me ae he af fe he He ae fe he ae afe he ae fe he 2h ae he he ae he he 2 he ake he a he he ake fe he ake ae Me ae he he ae fe he he ae he Me ae hc he ae he he ae he 2h ae ae 2h he hc ae ae ae he oe ie i a a 


*/ 


sync() 

{ 
sync_loc=0; 
while (sync_loc==0); 
return; 

/* 


46 He 2he he ae he he ae he he Ae ae He 2c ae he he ae he ah he ae 2h he ae af 2h ae a 2h 2 ae he ae age he ae ake he ae ae fe he ae fe he ae fe he he ae ae he ake he he ae he hc He ae she Se ae she ake ic kc 2k akc 
NAME : set_rob 

ARGUMENTS : Configuration to set robot’s location to. 

FUNCTION : to add set robot sequence to queue 


56 he He he Ke He He He He ee ee He He ae He ae He Ae he ee He he He He A he ee ee he he ee ee ee ee ee ae ke a oe ee a ee ke ak a kK KK 
*/ 

set_rob(pst) 

CONFIGURATION *pst; 


{ 
PATH_ELEMENT path; 
if(seq_status!=SSTOP) 
set_error(ECODE2); 
return; 


} 
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Nom_p->x = pst->x; 
nom_p->y = pst->y; 
nom_p->t = pst->t; 
nom_p->k = pst->k; 
length_norm = 0.0; 


path.type = SET_ROB; 

path.pc = (* pst); 

set_inst(path); 
last_robot_path_element.pc = (* pst); 
last_robot_path_element.type = SET_ROB; 
} 


/* end sequential.c */ 
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REREKEKSEKEKAKEKKKEKEEKKESEKESESEKSEESSERESKEKESKSEREKSESESERKESSERELKKEKESSSESESKECEESE 


track.c 
last update April 23, 1993 by Dave MacPherson 


BRHERKAAEAKRSEEAKAERKSEKEEKKESRKKKEKKKERSESSKSKSKRSKAKESESSEASESKSEKKKEKSEKHEKSSESREKES 


*/ 
#include “mml.h” 


extern int transition_point_test(); 


[RRR RRERERERERRE EGE RERERRE EERE EEEREREEEEEEEEEEEEEESEEEREREREREEEERERESE 


FUNCTION: read_rotate 

PARAMETERS: none 

PURPOSE: Reads a rotate instruction. Starts the robot 
rotating. The vehicle must be in the stop state in order 
top start rotating. 

RETURNS: void 

CALLED BY: 

CALLS: init_rotate() 

COMMENTS: 27 December 92 - Dave MacPherson 


HAH AA A HA A A HH HH RH A HH AH A HA RAK RAHA AEA EERE EEE AREA EEE EE EE / 
void read_rotate() 


drvel = racc * INTVL;/* rotation control */ 
th_g = vehicle.t + get_inst->c.x; 

goal_pst.t = th_g; 

goal_pst.x = vehicle.x; 

goal_pst.y = vehicle.y; 

if (th_g == vehicle.t) 


{ 
change_status(SSTOP); 


return; 

} 

if (th_g > vehicle.t) 
raccdrc = POSITIVE; 
else 


raccdrc = NEGATIVE; 
change_status(RMOVE); 
} /* end read_rotate() */ 


[RAMA RA AA AREA AAA AKER ER HHH BEAK HEE KEHE KK AAEK HHA EK KHER KE EH 


FUNCTION: cface20 

PARAMETERS: none 

PURPOSE: Reverses the current robot direction of travel. 
RETURNS: void 

CALLED BY: 

CALLS: norm 

COMMENTS: 27 December 92 - Dave MacPherson 
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ERREELEEEEEEEEEELESEREEEEREESEEESESEEESESRERESEEEEEEESEESEEEEESESERESE S / 
void cface2() 


int i; 
i = imaskoffQ; 


cur_t = norm(cur_t + PI); 
vehicle.t = norm(vehicle.t + PI); 
mv_direction = - mv_direction; 


imaskon(i); 
return; 


[RRMA HHH HARA AR HERA EAN EH REA AE ER EKER ER HER EERE EE EE EERE EEEEEEE EEE ES 


FUNCTION: limit 

PARAMETERS: double u 

PURPOSE: limit functon for delta_d input=(delta_d) 
output=(limited delta_d). 

RETURNS: void 

CALLED BY: update_kappa 

CALLS: none 

COMMENTS: 27 December 92 - Dave MacPherson 
HAA ARAMA HARA HARA AHH AHH HATE HRA HER HAH RETA REA EH EK EAE EK EERE EEE HERE REA S / 
double limit(u) 

double u; 


{ 

if(u > 2.0 * DIST_CONSTANT) return(2.0*DIST_CONSTANT); 
if (u < -2.0*DIST_CONSTANT) return(-2.0*DIST_CONSTANT); 
return(u); 

} /* end limit */ 


[RRR AAA AHA AHA HH HHA RR BH HAHAH A HH AA AT I HH AHH HH HH AH HH HAH HH AC 


FUNCTION: update_cubic_kappa 

PARAMETERS: vehicle, current_image 

PURPOSE: Main steering function for MML when using cubic spirals, 
uses a different ystar than other paths. 

RETURNS: void 

CALLED BY: stepper 

CALLS: limit(), 

COMMENTS: 6 Apr 93 - Bob Fish this is different than update_kappa() 
because Dr. K says ystar is different for cubic spirals than 

for lines and circles. 

HAAR HAA H AA AHR EAE HAHA EAE A RAAB EH HAE EAE HAE EEE EEE E EERE ERE REE ERE EE / 
double update_cubic_kappa(config, image) 

CONFIGURATION config; 

aia image; 
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register double dkappal; 
double cubic_ystar; /* ystar is different for cubic spiral than for lines&circles */ 


cubic_ystar = -(config.x-image.x)*sin(image.t) + (config.y-image.y)*cos(image.t); 


dkappal = -aa * (config.k - image.k) 
-bb * (norm( config.t - image.t)) 
-cc * limit(cubic_ystar); 


return config.k + dkappal * INTVL * vel_c; 
} /* end update_cubic_kappa */ 


KHRKKAKHKKAKAKKKHKRAKKKAKKKAKAKAAKKKKAKAAKKKHEKEKKKKKAKKEKKKEREAKHKKKKKEKKKEEKE 


FUNCTION: update_kappa 

PARAMETERS: none 

PURPOSE: Main steering function for MML. 
RETURNS: void 

CALLED BY: stepper 

CALLS: limit(), update_image() 
COMMENTS: 15 Feb 93 - Dave MacPherson 


HHH RHEE EEA AAA AAA EAE EAE RRS KEKE EK EEEEEERERERE EERE REREER ER ES ES / 


double update_kappa() 


register double delta_d; 
register double dkappal; 
double update_delta_d(Q); 


current_image = update_image(vehicle, current_robot_path.pc); 
delta_d = update_delta_d(vehicle, current_robot_path.pc); 


dkappal = -aa * (vehicle.k - current_image.k) 
-bb * (norm(vehicle.t - current_image.t)) 
-cc * limit(delta_d); 


return vehicle.k + dkappal * delta_dist; 
} /* update_kappa */ 


[RARER HEA AERA RE KEKE EAR KEKE EERE REESE RHEE AEH EA EERE RARE E KKK 


FUNCTION: update_delta_dQ) 

PARAMETERS: config, path 

PURPOSE: calculates the ystar for update_kappa() 

RETURNS: double 

CALLED BY: update_kappa 

CALLS: sin, cos 

COMMENTS: 15 Feb 93 - Dave MacPherson 

adie tdi daia dinate diate tiaintidiaadinndindediiatatndintediedialadiaiadidiadie diate diatietiatietiatiliaahiiadatindntatiatietattad | 
double update_delta_d(config, path) 

CONFIGURATION config; 




















CONFIGURATION path; 
double delta_d; 


delta_d = (-(config.x - path.x) * (path.k * 
(config.x - path.x) + 2 * sin(path.t)) - 
(config.y - path.y) * (path.k * 
(config.y - path.y) - 2 * cos(path.t))) / 
(1 + sqrt((path.k *(config.x - path.x)+ 
sin(path.t))* 
(path.k *(config. x - path.x)+ 

ath.t 


sin(path.t)) 
+ ((path.k * (config.y - path.y) - 
cos(path.t))* 
(path.k * (config.y - path.y) - 
cos(path.t))))); 


return delta_d; 
} /* end update_delta_d() */ 


[BRERA EEE EEE EE EEEE KEE EEE EE ER EKEEEE EERE EREKE EERE RE EERE ERE EERE EEE 
FUNCTION: transition_point_test 

PARAMETERS: image, tp 

PURPOSE: Tests to determine if the robot’s image 

is at or passed the transition point 
RETURNS: int (1 = at or passed the transition point, 0 = otherwise) 

CALLED BY: 

CALLS: 
COMMENTS: 15 Feb 93 - Dave MacPherson 

27 May 93 - Revision 1 Bob Fish, modified to check for already 

past the transition point. 
EREEERREREREEEREREEEEERREREREEEEREEEREREERERERESEREREBEEEEEEEREEEE EEE F / 
int transition_point_test(image, tp) 

CONFIGURATION image; 
POINT tp; 

{ 


/* Note: this needs to be modified, so that distance to the 
transition point is calculated using path distance vice 
EU_DIS. */ 
double current_dist; 
current_dist = EU_DIS(image.x, image.y, tp.x0, tp.y0); 
if (fabs(current_robot_path.pc.k) < ZERA) 

{ 
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/* current path is a straight line, check to see if close to or 
past the transition pt. */ 


if (current_dist < 1.0) 
i_am_here = 15; 
return 1; 

} 


else if (current_dist > last_dist+.05) 


i_am_here = 13; 
last_dist = current_dist; 
return 0; 


} * end if */ 


else /* path is a circle, use the transition point as the only test */ 


{ 

if (current_dist < 1.0) 
return 1; 

else 

return 0; 


} /* end if */ 


} /* end transition point test */ 


He He he ee HK A HH HAA RAKE AKA AKK KKK KE EEK KKK EEK EEK 


detect end motion 


then check next instruction 
Me he he He He He He he he Me He He He He He He He He ae He He Me He He he he He Me He Me He He he He he Me He he He Me ae He He He ke He 


*/ 

end_of_motionQ) 
rd ((msyn_m!=0) && (wsyn_q!=0)) 
msyn_m = 0; 
wsyn_q = 0; 
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RERKERREKEKKSKEKEKESAEKE CESKKESESSKEESEKESESESEE 


set length_stop 
SRKEKKEKKEKKKKKKAKSESKKEKKSEKSKAKSKEKKSESKKESESESKSESSESE 


*/ 
set_length_stop(class) 
int class; 


{ 
if(put_len==get_len)length_stop=INFINITE; 
else 


{ 
length_stop=*get_len; 
if(class==STOP) 


{ 
if(++get_len>tail_len)get_len=head_len; 
} 


[BREE AERA R REAR ERE AEA EA EE EE AEEEREREREEEEEEREREEEEER ERE EE EE EE EE EELS 


FUNCTION: disp_error 
PARAMETERS: code 

PURPOSE: Reports locomotion errors. 
RETURNS: void 


CALLS: 

COMMENTS: 27 December 92 - Dave MacPherson 

MAHA He HH A AH AHH A HR HA A A HHH HH HA ERR E EEE ER EEREREREREEEREEE ES FE / 
void disp_error(code) 

int code; 


{ 





switch (code) 


{ 

case ECODEO: 

r_printf(‘\n postures too close “); 

break; 

erase ECODE!: 

t_printf(‘\n bad cubic spiral specification “‘); 
break; 


case ECODE2: 
as a SSTOP function detected in moving state “); 
br ° 





default: 
ee undefined error code detected “‘); 
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[RESEEEESEEEEESESESESESESERESEEEESESERESESERESESESSESESESESESESEEESESSESES 


FUNCTION: change_status 
PARAMETERS: new_status 
PURPOSE: Reports new locomotion status when the status changes. 
RETURNS: void 
CALLED BY: 
CALLS: 
COMMENTS: 27 December 92 - Dave MacPherson 
BEREEEREEESELEEESESERESESESERESEE LESSEE ESSE SEESESEEESESESESESESESESES SE / 
void change_status(new_ status) 
a new_status; 
status = new_ status; 
if (status == SSTOP) wait_cnt = 100; 
/*changed wait_cnt from 400 to 100 31 May 1992*/ 


#ifdef SIM 
switch (status) 


case SSTOP: 
r_printf(“AnSSTOP\n”),; 
break; 


case SLINE: 
r_printf(“\AnSLINE\n”); 
break; 

case SBLINE: 
r_printf(“AnSBLINE\n”); 
break; 


case SFLINE: 
r_printf(‘AnSFLINE\n”); 
break; 

case SCONFIG: 
r_printf(“AnSCONFIG\n”); 
break; 


case SERROR: 
r_printf(‘\nSERROR\n”); 
break; 

default: 

break; 


} 
#endif 

} /* end change_status */ 
/* end track.c */ 
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SEEREEEESEEERESESKEESEESKEESERLEESESEEEEELESEEESEEE$EE6E6054606EEEE56EEEES4E% 


velocity.c 
last update May 24, 1993 by Dave MacPherson 
BERBEEESRESKSESESSKESKESKKEESESEESKEREESERESEERESESESEEEELEEESESSEESEEEEESEEESE 


*/ 
é#include “mml.h” 


RERREKKEERKEREEERKEKECHESEKESEEEEKESEESEEEESCEEREEESEEESEESEREKEKESEEKEEEESE 


FUNCTION: update_velQ 

PARAMETERS: none 

PURPOSE: Determines the current robot translational velocity. 
RETURNS: double 

CALLED BY: control() 

CALLS: rest_of_pathQ 

COMMENTS: 24 May 93 - Dave MacPherson 

TASK: Level 4 


BERGER EEEEEREEE ERE EERE EE EEE EERE EE ERERERE EE EEE EEREEEREREEEEEEEREEEEEEEE K / 


double update_vel() 
{ 


double vel_gg; /* temporary goal velocity */ 
double rest_of_pathQ; 


dvel = tacc * INTVL; 


if (status == SBLINE && 
2.0 * tacc * rest_of_path(current_robot_path,current_image) 
<= vel_c * vel_c) 


vel_c = max2(vel_c - dvel, 0.0); 
else 


{ 

vel_gg = min2(vel_g, WHEEL_MAX / (1 + TREAD / 2 * fabs(kappa))); 
if (vel_gg >= vel_c) 

vel_c = min2(vel_c + dvel, vel_gg); 

else 

vel_c = max2(vel_c - dvel, vel_gg); 


delta_dist = INTVL * vel_c; 
return vel_c; 
} /* end update_vel() */ 


[EERE ERE EERE EEA R ERA ERR REA AEEEEAHEERAEEEEEEEI 


FUNCTION: rest_of_pathQ) 
PARAMETERS: path, image 
PURPOSE: Determines the distance remaining on the 


295 











current_robot path. 

RETURNS: double 

CALLED BY: update_vel() 

CALLS: none 

COMMENTS: 24 May 93 - Dave MacPherson 

TASK: Level 4 
BEEEEESEEEEEESESEESSEESESESESESELSEESESEEEESEBESESEEESEESERERERESERES ES / 
double rest_of_path(path, image) 

PATH_ELEMENT path; 


ec aiaae image; 
switch(status) 
case BLINE: 


return ((path.pc.x - image.x)*cos(image.t) + 
(path.pc.y - image.y)*sin(image.t)); 
ie 


break; 
case CUBIC: 
break; 


*/ } 
} /* end rest_of_path */ 


[BERRA RER ERE R EERE HERE EEE EAE EERE EREEREEE EERE EREEE EEE EEE EEE EEE ERS 


FUNCTION: get_rotational_vel() 

PARAMETERS: none 

PURPOSE: Determines the required rotational robot 
velocity when the robot is rotating. 

RETURNS: rvel_c 

CALLED BY: 

CALLS: min2(), max2() 

COMMENTS: 22 Apr 93 - Dave MacPherson 


HEME EAA EERE ER EEEAAEEEEAAA AERA KEE EER ERE KEKE EREREERE EERE EERE EE EK / 
double get_rotational_vel() 


if (2.0 * racc * fabs(th_g - vehicle.t) > rvel_c * rvel_c) 


{ 

if (raccdrc == POSITIVE) /* CCW rotation */ 
tvel_c = min2(rvel_c + drvel, rvel); 

else /* clockwise rotation */ 

tvel_c = max2(rvel_c - drvel, -rvel); 


else /* robot rotational deceleration */ 
if (raccdrc == POSITIVE) 


{ 
if (vehicle.t < th_g) 
tvel_c = max2(rvel_c - drvel, 0.01); 
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else 


rvel_c = 0.0; 
change_status(SSTOP); 
oe 


} 
else /* CW rotation */ 


{ 

if (vehicle.t > th_g) 

rvel_c = min2(rvel_c + drvel, -0.01); 
else 


rvel_c = 0.0; 
change_status(SSTOP); 
read_inst(); 

} 

} 


return rvel_c; 


} /* end get_rotational_vel() */ 


/* end velocity.c */ 
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APPENDIX C. SONAR SOURCE CODE 


/* sonar2.c */ 


/* ultrasonic rangefinder functions */ 

/* 21 July 92 - Modified to discard sonar returns greater than 4.0 meter 
from the robot when building line segments - line 531 */ 

#include “mml.h” 

#include “cartography.h” 


#define print_flex(x,y) y = putstr(“ “, putstr(rtoae((double) (x), tmpstr, 4), y)) 
#define nl_flex(x) x = putstr(“An”, x) 


/*declaration of functions and returm values*/ 


extern double sonar(); 

extern void enable_sonar(); 

extern void disable_sonar(); 

extern double wait_sonar(); 

extern posit global (); 

extern void enable_linear_fitting(); 
extern void disable_linear_fitting(); 
extern void enable_data_logging(); 
extern void disable_data_logging(); 
extern void serve_sonar(); 

extern LINE_SEG *get_segment(); 
extern LINE_SEG *get_current_segment(); 
extern void set_parameters(); 

extern void enable_interrupt_operation(); 
extern void disable_interrupt_operation(); 
extern void calculate_global(); 

extern void linear_fitting(); 

extern void start_segment(); 

extern void add_to_line(); 

extern LINE_SEG *end_segment(); 
extern void build_listQ; 

extern void log_data(); 

extern void set_log_interval(); 

extern void wait_untilQ; 

extern void xfer_raw_to_hostQ; 

extern void xfer_global_to_host(Q); 

extern void xfer_segment_to_host(); 
extern void xfer_world_to_host(); 

extern void host_xferQ; 

extern void finish_segment(); 





[HERRERA ERE EEAE ERE RERES SEHR HERE EEE EE ERER ERE ER ER ERE EER EEE | 
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/* Procedure: sonar(n) 
it 


/* Description: returns the distance (in centimeters) sensed by the 
/* n_th ultrasonic sensor. If no echo is received, then a -1 is 
/* returned. If the distance is less than 10 cm, then a 0 is 
/* returned. 
* 


[BEPEREEEEEREEERESEEEESEEREDEESESEESEESESEESESESELESESESESESEEESESES EES / 


double sonar(n) 
int n; 


return sonar_table[n].d; 
} 


[BEREERERERREEAEEAEEEEEEEEEREEEEEEEE EEREEEESEEESEEESEREEEEEESEEESE EEE SS / 
i 


/* Procedure: enable_sonar(n) 
a 


/* Description: enables the sonar group that contains sonar n, which 
/* causes all the sonars in that group to echo-range and write data 
/* to the data registers on the sonar control board. Marks the n’th 
/* position of the enabled_sonars array to track which sonars are 
/* enabled. 

ie 


[RRR RARE AR RARER EEE ERE EERE ERE EER EREEREREEEE EES EEREEEEES EEE EEA EEEEE / 


void enable_sonar(n) 
int n; 


int i; 


i = imaskoff0; 
enabled_sonars[n]} = 1; 
switch (n) 


case 0: 

case 2: 

case 5: 

case 7: 

enabled = enabled | 0x01; 
break; 

case 1: 

case 3: 

case 4: 

case 6: 

enabled = enabled | 0x02; 
break; 

case 8: 
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case 9: 
case 10: 


case 11: 

enabled = enabled | 0x04; 
break; 

case 12: 

case 13: 

case 14: 

case 15: 

enabled = enabled | 0x08; 
oe 

*command_ptr = enabled; 

imaskon(i); 


[BRERA REEERE EERE REREEEEEEEEEERERE EERE EES EEE EESEEEEESERERERESERESEE SS / 
* 


/* Procedure: disable_sonar(n) 
bd 





/* Description: removes the sonar n from the enabled_sonars list. If 
/* sonar n is the only enabled sonar from it’s group, then the 
/* group is disabled as well and will stop echo ranging. This has 
/* benefit of shortening the ping interval for groups that remain 
/* enabled. 
te 


[BRERA ARREAREREAE EEE ERERE REE EERE EERE REE E EERE EEE EEE ERE EEE EERE EE EE / 


void disable_sonar(n) 
int n; 


int i, Cc; 
char mask; 


i = imaskoffQ; 
enabled_sonars[n] = 0; 
switch (n) 


case 7: 

c = enabled_sonars[0] + enabled_sonars[2] + 
enabled_sonars[{5] + enabled_sonars[7]; 

if (c == 0) 

enabled = enabled & Oxfe; 
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c = enabled_sonars[1]} + enabled_sonars[3] + 

ane a + enabled_sonars[6]; 

if (c == 0) 

enabled = enabled & Oxfd; 

break; 

case 8: 

case 9: 

case 10: 

case 11: 

c = enabled_sonars[8] + enabled_sonars[9] + 

enabled_sonars[10] + enabled_sonars[11]; 
Cc== 


enabled = enabled & Oxfb; 
break; 


case 12: 

case 13: 

case 14: 

case 15: 

c = enabled_sonars[12] + enabled_sonars[13] + 
enabled_sonars[ 14] + enabled_sonars[ 15]; 

if (c == 0) 

enabled = enabled & Oxf7; 

break; 

} 


*command_ptr = enabled; 
imaskon(i); 


[RAHN HH HARRAH HHH ERA AA HEH TAH EBA A AHH HEHE TEA ERE EERE HERAT / 
/* 
/* Procedure: wait_sonar(n) 
/* 
/* Description: waits in a loop until new data is available for 
/* sonar n. 
ae 


[BRERA AAR AEA REAR A HEE KER AK ERE KAA EERE KEK EERE EE EAA EE EEE / 


double wait_sonar(n) 
int n; 


int a = 0; 
return sonar_table[n].d; 
[BRRRRAER EERE ERE REE EE ARERR EERE ERE EEE HEE EEE ARE REN ERE REE EE EER ERE ERE ERE / 


[* 
/* Procedure: global(n) 
/* 
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/* Description: returns a structure of type posit containing the global 
/* x and y coordinates of the position of the last sonar return. 


[REEEREREEEEEEEEESEREEESEREEEEEEEEREEEEEEEEEESEEEESEEEEESEEEEESS SEES ESS S/ 


posit global (n) 
int n; 
{ 


posit answer; 


if (sonar_table[r].global == 0) 

calculate_global(n); 

answer.gx = sonar_table[n].gx; 

answer.gy = sonar_table[n].gy; 

answer.psi = sonar_table[n].t + sonar_table[n].axis; 
return answer; 


} 


[BARRA HAA AAAI HERA AEE EAA EEE ARK EEE REE AEE ERA E EEE EERE EERE AHERN / 
/* 
/* Procedure: enable_linear_fitting(n) 
/* 
/* Description: causes the background system to gather data points 
/* from sonar n and form them into line segments as governed by 
/* the linear fitting algorithm. 
i 





[RRA A AAA RR EAA AH ARE RAH HEE ERA EE EERE HEH BER HEH KT KKH ER AHA / 


void enable_linear_fitting(n) 
int n; 
sonar_table[n].fitting = 1; 
sonar_table[n].global = 1; 
} 


[RRA AAR IR HAA RAR EHH AEA HH HRA HAHAH HEAR EH NH HEHE EE HEH HEAR HEH RA EE / 
/* 

/* Procedure: disable_linear_fitting(n) 

/* 


/* Description: causes background system to cease forming line 
/* segments for sonar n. 

/* Will also disable the calculation of global coordinates for 

/" that sonar if data logging of global data is not enabled. 


[BRRRREARR ER EERE ERE EEE EEE REA REE EE HHA EE EERE EH ERA HE EEA EAE / 


void disable_linear_fitting(n) 
int n; 


sonar_table[n].fitting = 0; 
if (sonar_table[n].filetype[{1] == 0) 
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sonar_table{n}.global = 0; 





[BERREEEEEEEEEEEESEERESEREEEEESSSESSEEEEEESESEEESESESESESESESESE SER ES ES / 


e 
/* Procedure: enable_data_logging(n,filetype,filenumber) 
f 


/* Description: causes the background system to log data for sonar (n) 
/* to a file (filenumber). The data to be logged is specified by an 
/* integer flag (filetype). A value of 0 for filetype will cause raw 
/* sonar data to be saved, 1 will save global x and y, and 2 will 
/* save line segments. The filenumber may range between 0 and 3 for 
/* each of the three types, providing up to 12 data files. Example: 
/* enable_data_logging(4,1,0); 
/* will cause raw data from sonar #4 to be saved to file 0, while: 
/* enable_data_logging(7,2,0); 
/* will cause segments for sonar #7 to be saved to file 0. 
bd 


[BERRA REAE EEE EERE EERE EEEEREE EERE SERRE EEE EB EEE EERE EEE EER EEE EEE EEE € / 


void enable_data_logging(n, filetype, filenumber) 
int n, filetype, filenumber; 
{ 


if (filetype == 1) 
sonar_table[n].global = 1; 


sonar_table[n].filetype[filetype] = 1; 
sonar_table[n] filenumber[filetype] = filenumber; 
} 


[HERRERA EERE ERE EERE EEER EEE ERERE EERE EE REREEEEERE EEE EEEEEREN HE EEE / 
Ke 


/* Procedure: disable_data_logging(n,filetype) 
i* 


/* Description: causes the background system to cease logging data of a 
/* given filetype for a sonar n. 
ie 


[BERRA RAEAAEAEARE REE KERR ERE EEREREEEREEEEREEEERERERE ERE ERE EERER ER ERE / 


void disable_data_logging(n, filetype) 
int n, filetype; 


if ((filetype == 1) & & (sonar_table[n].fitting == 0)) 
sonar_table[n].global = 0; 


sonar_table[n).filetype[filetype] = 0; 


[BEREREREAAAEAAEAEE ERE EREREEEER ERE REREEEEEEREEERERE REE SEEKER ER EE EEE EEE / 
Ke 


/* Procedure: serve_sonar(x,y,t,ovfl,data 1 ,data2,data3,data4, group) 
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f* 

/* Description: this procedure is the “central command” for the 
/* control of all sonar related functions. It is linked with 

/* the ih_sonar routine and loads sonar data to the sonar_table 

/* from there. It then examines the various control flags in the 

/* sonar_table to determine which activities the user wishes to 
/* take place, and calls the appropriate functions. This procedure 
/* is invoked approximately every thirty milliseconds by an 

Uh interrupt from the sonar control board. 


[RRRREEEEREEREEESEEEEREEEEEEEEEEEEESEREESESE SESE EEEEERE SEES EEE EEEEEES SS / 


void serve_sonar(x, y, t, ovfl, data4, data3, data2, datal, group) 
double x, y, t; 

int ovfl, data4, data3, data2, datal, group; 

{ 


inti, n; 
int data[4]; 
int ovfl_mask = 8; 


data[0] = data1; 
data[1] = data2; 
data[2] = data3; 
data[3] = data4; 


for (i = 0; i < 4; i++, ovfl_mask /= 2) 
n = group_array[group][i];/* n = sonar number */ 


if (ovfl_mask & ovfl) 
sonar_table[n].d = -1.0; 


else if (data[i] < 100) 
sonar_table[n].d = 0.0; 
else 


sonar_table[n].d = (double) data{i] / 10.0; 
sonar_table[n].x = x; 

sonar_table[n].y = y; 

sonar_table[n].t = t; 

if (sonar_table[n].global == 1) 
calculate_global(n); 

if (sonar_table[n}.fitting == 1) 
linear_fitting(n); 

if (sonar_table[n].filetype{[0] == 1) 

log_data(n, 1, sonar_table[n].filenumber[0], 0); 
if (sonar_table[n].filetype[1] == 1) 

log_data(n, 2, sonar_table[n].filenumber[1], 0); 


} /* serve_sonar() */ 
[5#ERERRAKEAREREREREREERERE EERE EEEREEERERERERERERE REE ERERERER ERE EE EEE AE / 


/* 
/* Procedure: get_segment(n) 
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/* 

/* Description: returns a pointer to the oldest segment on the linked 
/* list of segments for sonar n; i.e. the record at the head 

/* of the linked list. It is destructive, thus subsequent calls 

/* will return subsequent segments until the list is empty. This is 

/* accomplished by first copying the contents of the head record 

/* into a temporary record called segstruct and then freeing the 

/* allocated memory for the head record. The pointer returned is 

/* actually a pointer to this temporary storage. If get_segment is 

/* called on an empty list a null pointer is returned. 


[BEREEEEEEEEEESEEEEEEERESEREEEERESEERESESEEEEEEEREEEERESERESERESEE ES EES / 


LINE_SEG *get_segment(n) 
int n; 


LINE_SEG *ptr; 
int index; 


index = seg_list_head[n]; 
if (index == -1) 

ptr = NULL; 

else 


{ 
ptr = &seg_list{[n] [index]; 
ee = (index < 4) ? (index + 1) : 0; 


return ptr; 
} 


[BARRERA AAR EAE EERE EEE AEE EERE EEA EEE EE EEE AEE EEE EEE EER EER EEE EER EET / 
fd 


eB Procedure: get_current_segment(n) 

/* Description: returns a pointer to the segment currently under 
/* construction if there is one, otherwise returns null pointer. 

/* This is accomplished by calling end_segment, copying the data 
/* into segstruct and then returning a pointer to segstruct. The 

is memory allocated by end_segment is then freed. 


[RRERRRREE EERE EERE ERE ERERAERERER ERASER ERE RE EERE EREEE EE HERRERA EE | 


LINE_SEG *get_current_segment(n) 
int n; 


LINE_SEG *ptr; 
ptr = end_segment(n); 


return ptr; 
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[/PERERESESEEREDESESESESEEEESESESESSESESSESESESESEESESESE SES ES ESEESEESS / 


/* 
i Procedure: set_parameters(c1,c2,c3) 


/* Description: allows the user to adjust constants which control 
/* the linear fitting algorithm. C1 is a multiplier for standard 

/* deviation and C2 is an absolute value; both are used to 

/* determine if an individual data point is usable for the 

/* algorithm. C3 is a value for ellipse thinness; it is used to 

/* determine the end of a segment. Default values are set in main.c 
/* to 3.0, 5.0, and 0.1 respectively. 


[RRRRREREEREREEEREREEESEEEEEEEEEREEEEEESERSEEEERESEEEEEELE ESE SEES EE EE / 


void set_parameters(cl, c2, c3) 


double cl, c2, c3; 
Cl=cl; 
C2 =c2; 

C3 =c3; 


[RRRRRREKAEREREEEEREREREREREREEEERERERE ER ERERERE ES ELERESER ERE EEE EEE EHH / 
/* 
/* Procedure: enable_interrupt_operation() 
/* 
/* Description: places sonar control board in interrupt driven mode. 
Ik 


[RRR REBAR AAA A ERE RARER REE RE ERE EERE ERER ERE ERE EE ES EEE EERE RHEE EH / 


void enable_interrupt_operation() 


*BIM_ptr = *BIM_ptr | 0x10; 
[RRR RR ERA H HAH E AERA EER EEA ERE EEE EEE EA HEE EEA EEE EEA REA EERE EE EHH HEE / 
/* 
/* Procedure: disable_interrupt_operation() 
i* 
/* Description: stops interrupt generation by the sonar control 
/* board. A flag is set in the status register when data is ready, 
/* and it is the user’s responsibility to poll the sonar system 
/* for the flag. 


[FRRERREEEEREREERERERERERE 2 EE EEEREEEEEEEEREERERE EERE SESE ERESE EERE EEE / 


void disable_interrupt_operation() 
*BIM_ptr = *BIM_ptr & Oxef; 
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[BESEREEEESEESEESESERESESESESESE SESE SESE EESESESEESESSSESESESESESESESS / 


‘ha 
i Procedure: calculate_global(n) 


/* Description: this procedure calculates the global x and y coordinates 
/* for the range value and robot configuration in the sonar table. 
/* The results are stored in the sonar table. 


[PERREERESREESESEESESEEEESESESEEEESESERESRESESESEEESESESESESSEEEESE ESE S / 


void calculate_global(n) 
int n; 


double Lx, ly, gt, range, phi, axis, offset; 


gt = sonar_table(n].t; 

range = sonar_table[n].d; 

phi = sonar_table[n].phi; 

axis = sonar_table[n].axis; 
offset = sonar_table[n].offset; 


if (range == -1) 
range = 9999; 
k= sonar_table[n].x + (cos(gt + phi) * offset);/* global x position of 
* sonar */ 
ly= sonar_table(n].y + (sin(gt + phi) * offset);/* global y position of 
* sonar */ 
sonar_table[n].gx = 1x + (cos(gt + axis) * range);/* global x position of 
* range */ 
sonar_table[n].gy = ly + (sin(gt + axis) * range);/* global y position of 
* range */ 


} 


[BRA ARA AAR RE EERE ER EERE EEE ERE EERE EEE EERE SE EERE REE EERE EEE EE SE / 
ir 

/* Procedure: linear_fitting(n) 

te 


/ 
/* Revised by Y. Kanayama, 07-07-93 
/* 


/* Description: this procedure controls the fitting of point data to straight 
/* line segments. First it tests if the new coming point is not far from 

/* the fitted line. If the test is passed, the point is added to test 

/* if the thinnes test is passed. If it is passed, the addition is 

/* finalized. 

/* If any of the tests fail, the line segment is ended and a new one 

/* started. The completed line segment is stored in a data structure 

‘ called segment, and segments are linked together in a linked list. 


[8 EEERERREEAE AREER EREEESEEEEEEEE ERE REREREREREEE EEE EEEEEEEEEREREREEER ES | 
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void linear_fitting(n) 
int n; 


double x, y, m00, m10, m01, m20, m11, m02; 
double alpha, r, sigma, delta; 
LINE_SEG *finished_segment, 


if (sonar_table[{n].d < 9.3 Ii sonar_table[n].d > 409.0) 


finish_segment(n); 

start_segment(n); 

return; 

} 

*/ 

x = sonar_table[n].gx;/* temporary moments */ 
y = sonar_table{n].gy; 

m00O = segment_data[n].m00; 

if (m00 < 1.5) 


{ 
add_to_line(n, x, y); 
retum; 


} 

r = segment_data[n]. a pa 7 
alpha = segment_ data[n). alph 

delta = fabs(r - x + costaipha) - y * sin(alpha)); 


sigma = sari( segment_data[n}.m_major / (m00 - 1.0)); */ 
if (delta > C2) 


{ 

if (mOO > 10.0) */ 
finish_segment(n); 
start_segment(n); 
add_to_line(n, x, y); 
return; 

} else 


{ 
add_to_line(n, x, y); 
return; 


} 
/* end linear_fitting */ 


[8HEREREREREEEEEEEEREEEREREREREE EEE EREREERESEREEE EEE EEEEE EEE EER EREREE E / 


/* 
/* Procedure: start_segment(n) 
I haa 


/* Description: this procedure establishes a new line segment with the three 
/* data points contained in segment_data[n].init(x and y). It writes 
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/* the appropriate data to the interim values in segment_data(n]. 


[BESRHSSSESEERESEESESESERESESEESESESESERESESESESESSEESESERESESSEESESE SES / 


void start_segment(n) 
int n; 


segment_data[n]}.m00 = 0.0; 
segment_data[n].m10 = 0.0; 
segment_data[n].m01 = 0.0; 
segment_data[n].m20 = 0.0; 
segment_data[n]}.m11 = 0.0; 
segment_data[n].m02 = 0.0; 


[REREREREREREREREEESAEEEEEEEEEEEEEEEEEREREREEEEREREERESEEEEEESEREEEEEE SE / 
tha 
/* Procedure: add_to_line(n, x, y) 


/* Description: this procedure calculates new interim data for the line segment 
/* and stores it in segment_data{n]. It also changes the end point values to 
/* the point being added. 


[BEREREREEEREREREEEEREEEERESEREEEEEEEEEEREREEESEEEEEERERESEEESEEEREEESSE S / 


void add_to_line(n, x, y) 
int n; 

double x, y; 

{ 


double m00, m10, m01, m20, m11, m02; 
double m_major, m_minor, d |_major, d_ minor, alpha, r, rho; 
double mux, muy, mm20, mm11, mm02; 


m00 = segment_data[n].m00 += = 
m10 = segment_data[n].m10 += x 

m01 = segment_data[n].m01 += y 

m20 = segment_data[n].m20 += SqRon 
m11 = segment_data[n].m11 += x * 
m0Q2 = segment_data[n].m02 += SOR): 


4 (m00 < 1.5) 


segment_data{n].startx = x; 
lanier a =y; 


mux = m10 / m00; 
muy = m01 / m00; 
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mm20 = m20 - SQR(m!10) / m00; 
mm11 = m11 - m10 * m01 / m00; 
mm02 = m02 - SQR(m01) / m00; * 


segment_data[n].m00 = m00; 
segment_data[n].m10 = m10; 
segment_data[{n].m01 = m01; 
segment_data[n].m20 = m20; 
segment_data{n].m11 = m11; 
segment_data(n].m02 = m02: 


ca > 1.5) 


é m_major = (mm20 + mm02) / 2.0 - sqrt((mm02 - mm20) * (mm02 - mm20) / 4.0 + 
GaGa 1)); 
m_minor = (mm20 + mm02) / 2.0 + sqrt((mm02 - mm20) * (mm02 - mm20) / 4.0 

+ SQR(mm!1 1)); 

d_major = 4.0 * sqrt(fabs(m_minor / m00)); 

d_minor = 4.0 * sart(fabs(n. major / m00)); 

tho = d_minor / d_major; 

alpha = atan2(-2. 0 * mm11, (mm02 - mm20)) / 2.0; 

r = mux * cos(alpha) + muy * sin(alpha); 


*/ 


segment_data(n).alpha = alpha; 
segment_data[n].r = r,; 
segment_data[n].m_major = m_major; 
segment_data[n].m_minor = = m_minor, 
segment_data[n].d |_major = =d |_major, 
segment_data[n].d_minor = d_minor; 
segment_data[n].rho = rho; 
segment_data[n].endx = x; 
ima aad =y; 


} 


[RHRREREREEREERAEEEREREREEEEREREREEREREEEEEREREEEE EERE EERE EERE EEE EEE EE / 
/* 
/* Procedure: end_segment(n) 


/* Description: this procedure allocates memory for the segment data structure, 
/* loads the correct values into it and returns a pointer to the structure. 
te 


[ARAERERERREREEERERERERERESESEEEREEEEREEEEEEEEEEEEEEEEEREEEREEE EKER EER ES / 


LINE_SEG *end_segment(n) 
int n; 


LINE_SEG *seg_ptr; 
double startx, starty, endx, endy, delta, alpha, r, length; 


seg_ptr = &segstruct; 
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startx = segment_data[n].startx; 
Starty = segment_data[n].starty; 
endx = segment_data[n].endx; 
endy = segment_data{n].endy; 
alpha = segment_data[n).alpha; 
r = segment_data{n].r; 
delta = startx * cos(alpha) + starty * sin(alpha) - r; 
startx = startx - (delta * cos(alpha)); 
Starty = starty - (delta * sin(alpha)); 
delta = endx * cos(alpha) + endy * sin(alpha) - r, 
endx = endx - (delta * cos(alpha)); 
endy = endy - (delta * sin(alpha)); 
length = sqrt(SQR(startx - endx) + SQR(starty - endy)); 


seg_ptr->headx = startx; 

seg_ptr->heady = starty; 

seg_ptr->tailx = endx; 

seg_ptr->taily = endy; 

seg_ptr->alpha = alpha; 

seg_ptr->r =r; 

seg_ptr->length = length; 

seg_ptr->dmajor = segment_data[n].d_major, 
seg_ptr->dminor = segment_data[n].d_minor; 
seg_ptr->sonar = n; 


return seg_ptr; 
} /* end end _ segment */ 


[BRERRRREEREREAEREEEEEEEEEEEEEEERERERERERERERESERESEEEEEEEEEERERE EEE SE / 
/* 

/* Procedure: build_list(ptr, n); 

| haa 


/* Description: this function accepts a pointer to a segment data structure and 
/* a sonar number, and appends the segment structure to the tail of a linked 
/* list of structures for that sonar. 


[BRRRRERERAAEREREREEEEREREREREE EERE BEES EE EREREREEEEEEREEE EEEREEEREEEEEE / 


void build_list(ptr, n) 
int n; 

LINE_SEG *ptr, 

{ 


int next; 


if (seg_list_tail{n] == -1) 

seg_list_head[n] = 0; 

next = (seg_list_tail[n] < 4) ? ++seg_list_tail(n] : 0; 

if (next == seg_list_head[n]) 

seg_list_head[n] = (seg_list_head[n] < 4) ? ++seg_list_head[n] : 0; 
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seg_list{n][next) = *ptr; 
if (sonar_table[n].filetype{2] == 1) 
log_data(n, 3, sonar_table[n].filenumber(2], next); 


[HEEREEERERESEESESEEESESEEESERERESERESE EES EEEEEEEESESERESESESE SES ESE EES / 


if 
/* Procedure: log_data(n, type, filenumber,i) 
/f* 


/* Description: this procedure causes data to be written to a file. The filenumber 
/* designates which “column” (0,1,2, or 3) of a two dimensional array for 

/* that type of data is used. The data array and a counter for each column 

/* forms the data structure for each type. The value of i is used to index 

/* the seg_list array for storing line segments. 


[RARRERREREEEEREREREEREREREEEEERESESEESEES ERE EES EEE EERESEEREES EEE SES ES / 


void log_data(n, filetype, filenumber, i) 
int n, filetype, filenumber, i; 
{ 


int count, interval, next; 
switch (filetype) 
{ 


case 1: 

count = raw_data_log{filenumber] count; 
interval = sonar_table[n].interval; 

oo < MAXRAW) && !(count % interval)) 


next = raw_data_log[filenumber].next; 
raw_data_log{filenumber].darray[next] = sonar_table[n].d; 
raw_data_log[filenumber].xarray[next] = sonar_table[n].x; 
raw_data_log[filenumber].yarray[next] = sonar_table[n].y; 
raw_data_log([filenumber].tarray[next] = sonar_table[n].t; 
raw_data_log[filenumber].next += 1; 


raw_data_log[filenumber].count += 1; 

break; 

case 2: 

count = global_data_log{filenumber].count; 

interval = sonar_table[n].interval; 

if (count < MAXGLOBAL) && !(count % interval)) 


next = global_data_log{filenumber].next; 
global_data_log[filenumber].xarray[next] = sonar_table[n]}.gx; 
global_data_log[filenumber].yarray{next] = sonar_table[n].gy; 
global_data_log{filenumber].next += 1; 


global_data_log[filenumber].count += 1; 


break; 
case 3: 
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count = segment_data_log[filenumber].count; 
if (count < MAXSEGMENT) 


segment_data_log{filenumber].array[count] = seg_list(n)[i); 


segment_data_log[filenumber]}.count += 1; 
break; 
} 


[BERRERESEEREEEEEEEEEEESESESERESERESERESESEESERESESESERESESESSESESESES S/ 


‘bi 
/* Procedure: set_log_interval(n,d) 
i* 


/* Description: this procedure allows the user to set how often the sonar system 
/* writes data to the raw data or global data files. The interval d is stored 

/* at sonar_table[n], and one data point will be recorded for every d data 

/* points sensed by the sonar. Default value for interval d is 13, which for 

/* a speed of 30 cm/sec and sonar sampling time of 25 msec should record a 

/* data point every 10 cm. 


[BRRRRRREEERERERR EERE EEE EREEEEEEEEAEEEEEREEEEEEEEEREREREEREEEREEEEEEESS / 


void set_log_interval(n, d) 
int n, d; 
{ 
sonar_table[n].interval = d; 


[BERRA EREAA EERE HERES HERA HERE RE EER BREE EERE EE EEE EREREREREEEEEREEES EE / 
/* 

/* Procedure: wait_until(variable,relation, value) 

i 

/* Description: this procedure will delay it’s completion (and thus the continuance 
/* of the program it’s embedded in) until the variable achieves the relation with 

/* the value specified. For example, presume the robot is traveling along the x 

/* axis. If the user wants the robot to begin redording sonar data when the x 

/* position of the robot exceeds 500 cm., he would insert this command after the 

/* move command: 

/* wait_until(X,GT,500.0); 

/* enable_sonar(sonar number); 

/* The variable are predefined as X, Y, A and DO through D11, and correspond to 
/* the robot’s current x position, y position, alpha, and range from sonars 0 

/* through 11. Relations are predefined as GT, LT and EQ corresponding to greater 
/* than, less than and equal to. Value may be any number expressed as a double 

r or the predefined values PI, HPI, PI34, P14, or DPI. 


[RRRRREERREREEAE EERE EERE EEEEEEERERERES ERE EERE ERERERE EERE EERE EEREREEE / 
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void wait_until(variable, relation, value) 
int variable, relation; 
double value; 


double *ptr; 
double theta; 
int test, item; 


if (variable == 14) && (relation == 17)) 
test = (int) (1000.0 * value); 

else if (relation == 17) 

test = (int) (value); 


switch (variable) 


case 11: 

ptr = &sonar_table[variable].d; 
break; 

case 12: 

ptr = &vehicle.x; 

break; 

case 13: 

ptr = &vehicle.y; 

break; 

case 14: 

theta = 1000.0 * vehicle.t; 
ptr = &theta; 

break; 


} 
switch (relation) 


case 15: 
do 


item = *ptr; 
while (item <= value); 
break; 


case 16: 
do 
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{ 
= = *ptr; 


while (item >= value); 


case 17: 
do 


{ 

a = (int) *ptr; 
while (item != test); 
break; 


} 


} 


[BRERREEEERESEEREEERESEEESERESESESESEEESESEREREEE REE EESESEEEEEES ELE SERS / 
/* 

/* Procedure: xfer_raw_to_host(filenumber, filename) 

i* 

/* Description: this function allocates memory for a buffer and then converts a raw data 
/* log file to a string format stored in the buffer. It then calls host_xfer to send 

/* the string to the host. When that transfer is complete, it frees the memory it 

/* allocated for the buffer. Filename must be entered in double quotes ( “dumpraw” 

/* for example). 

[* 


[BEEEEREEREREEEEEEERE EERE EEEEEEERERESEREREREEEEREREREEEEEEREEEEE ERE EES / 


void xfer_raw_to_host(filenumber, filename) 
int filenumber; 
char *filename; 


char *rbuffer; 
char *start; 
int i, c, j; 


i = raw_data_log[filenumber].next; 
c = 20 + (i * 33); 

rbuffer = malloc(c); 

Start = rbuffer; 

ce = 0; j <i; j++) 


print_flex(raw_data_log{filenumber].darray[j], rbuffer); 
print_flex(raw_data_log{filenumber].xarray[j], rbuffer); 
print_flex(raw_data_log[filenumber].yarray[j], rbuffer); 
print_flex(taw_data_log[filenumber].tarray[j], rbuffer); 
nl_flex(rbuffer); 


} 

putb(0’, rbuffer); 

rbuffer = start; 
host_xfer(rbuffer, filename); 
free(rbuffer); 
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[RRR HR RAH E AREER AREER EEE REEEREEERESEEEERESERESES EEE EERE EE ERE EE EEES EES / 


r* 
/* Procedure: xfer_global_to_host(filenumber, filename) 
/* 


/* Description: this function performs the same function as xfer_raw_to_host, but for 
/* global data vice raw data. 
* 


[RRR EHH REE ERR TERRE EERE EERE EEE EER ERESEREEEER ERE EE EE EEE EES / 


void xfer_global_to_host(filenumber, filename) 
int filenumber; 
char *filename; 
{ 
char *gbuffer; 
char *start; 
inti, c, j; 


i = global_data_log[tiicnumber].next; 
c=20 + (i * 17); 

gbuffer = malloc(c); 

start = gbuffer; 

for (j = 0; j <i; j++) 


print_flex(global_data_log[filenumber].xarray[j], gbuffer); 
print_flex(global_data_log[filenumber].yarray[j], gbuffer); 
ni_flex(gbuffer); 


} 
putb(\0’, gbuffer); 
gbuffer = start; 
host_xfer(gbuffer, filename); 
free(gbuffer); 

} 


[RERRRESPREERECEELARES ELESEEEESERELEEE ES CESSES SESS EERE ESS EE EREE ES ELSES ES | 
/* 
/* Procedure: xfer_segment_to_host(filenumber,filename) 
/* 
/* Description: this function performs the 
/* same function as xfer_raw_to_host, but for 
/* segment data vice raw data. 
Kk 


[EER A ARR EII AA HERA AA HH ERE I A HH EHH ATR HHH AH HH EHH AHH RK / 


void xfer_segment_to_host(filenumber, filename) 
int filenumber; 
char *filename; 


{ 


char *segbuffer; 











char *start; 
int i, c, j; 


i= segment. ~ _log[filenumber].count; 
c = 20 + (i * 77); 

segbuffer = malloc(c); 

Start = segbuffer; 

ne = 0; j <i; j++) 


print_flex(segment_data_log[filenumber].array[j].headx, segbuffer); 
print_flex(segment_data_log{filenumber].array[j].heady, segbuffer); 
print_flex(segment_data_log{filenumber]. array{j).tailx, segbuffer); 
print_flex(segment_data_log[filenumber].array[j).taily, segbuffer); 
nl_flex(segbuffer); 
print_flex(segment_data_log[filenumber].array[j].alpha, segbuffer); 
print_flex(segment_data_log{filenumber].array[j].r, segbuffer); 
print_flex(segment_data_log[filenumber].array[j].length, segbuffer); 
print_flex(segment_data_log[filenumber].array[j].dmajor, segbuffer); 
print_flex(segment_data_log[filenumber].array[j].dminor, segbuffer); 
nl_flex(segbuffer); 


putb(‘\0’, segbuffer); 
segbuffer = start; 
host_xfer(segbuffer, filename); 
free(segbuffer); 
} 
3 He he ae Me ae Me ae He He Me ae He He He Be He He He he He Me Me Me He Me Me ae Me Me he He Me Me he He he Me He He Me ae he ae he Me he Me she ae Me ae Me Ae Me ae he ae he ke he Ae he Me ae ee ae 2k aK 
MAAK / 
/* 


/* Procedure: xfer_world_to_host(world) 
fi] 


/* Description: this function transfers the edges of 
/* a partial world to the host. 
/* 


Me Me He ae he ae He he he He he ae Me ae He ake ae He he ae ae he ae he he ae ae he ae he he ee Me he He He ake a he he ae he he a 2 he ae ae he ae ac ie ae He he ae 2 3 ae ae a a a ke ee ok ie 
#REERK/ 


void xfer_world_to_host(PW, filename) 
Map_World *PW; 
‘a *filename; 
char *segbuffer,; 
char *start; 
int i, c, j, k; 
int 1 = 0; 
Map_Polygon *current_polygon; 
EDGE *current_edge; 
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/* 


ad | 


} 


i = PW->boundary->degree; 


current_polygon = PW->hole_list; 
for (k = 1; k < PW->degree; k++) 
{ 


i += current_polygon->degree; 
current_polygon = current_polygon->next; 


c = 20 + (i * 43); 
segbuffer = malloc(c); 
start = segbuffer; 


/* Put the boundary polygon edges in the buffer */ 
current_edge = PW->boundary->edge_list; 

for (j = 0; | < PW->boundary->degree; j++) 

{ 


print_flex(current_edge->v 1.x, segbuffer); 
print_flex(current_edge->v1.y, segbuffer); 
nl_flex(segbuffer); 
print_flex(current_edge->v2.x, segbuffer); 
print_fle:(current_edge->v2.y, segbuffer); 
if (current_edge->type == REAL) 
print_flex(1.0, segbuffer); 

else 

print_flex(0.0, segbuffer); 
nl_flex(segbuffer); 

nl_flex(segbuffer); 

current_edge = current_edge->next; 


/* Put the hole polygon edges in the buffer */ 
for (j = 0; j <1; j++) 
{ 


print_flex(segment_data_log[filenumber].array[j].headx, segbuffer); 
print_flex(segment_data_log[filenumber].array[j].heady, segbuffer); 
print_flex(segment_data_log[filenumber].array[j].tailx, segbuffer); 
print_flex(segment_data_log[filenumber].array[}].taily, segbuffer); 
nl_flex(segbuffer); 

} 


putb(\0’, segbuffer); 
segbuffer = start; 
host_xfer(segbuffer, filename); 
free(segbuffer); 


[FARA REAAEEERA AERA EEE EERE EE EREEE EEE ERERE EE ERE EE EEE EEE ERE EERE EE EE / 


[* 


/* Procedure: xfer_real_boundary_edges_to_host(world) 
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thd 
/* Description: this function transfers the real edges of 
/* the boundary polygon of a partial world to the host. 


[PRBRREEEREESEEEEEESESESEESESESESESESSESESESEESESESESESESEEES ESE SESE ESSE / 


void xfer_real_boundary_edges_to_host(PW, filename) 
Map_ World *PW; 
char *filename; 


char *edgebuffer; 

char *start; 

int c, j, k; 

int count = 0; 

Map_ Polygon *current_polygon; 
EDGE *cur ant_edge; 


current_edge = PW->boundary->edge_list; 
for (k = 1; k < PW->boundary->degree; k++) 


{ 

if (current_edge->type == REAL) 
++count; 

current_edge = current_edge->next; 
} 


c = 20 + (count * 35); 
edgebuffer = malloc(c); 
start = edgebuffer; 


/* Fut the boundary polygon edges in the buffer */ 
current_edge = PW->boundary->edge_list; 
for (j = 0; j < PW->boundary->degree; j++) 


{ 
i (current_edge->type == REAL) 


print_flex(current_edge->v1.x, edgebuffer); 
print_flex(current_edge->v1l.y, edgebuffer); 
nl_flex(edgebuffer); 
print_flex(current_edge->v2.x, edgebuffer); 
print_flex(current_edge->v2.y, edgebuffer); 
nl_flex(edgebuffer); 

nl_flex(edgebuffer); 

current_edge = current_edge->next; 


} 
putb(‘\0’, edgebuffer); 
edgebuffer = start; 


host_xfer(edgebuffer, filename); 
free(edgebuffer); 
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} /* end xfer_real_boundary_edges_to_host() */ 


[PRRREEREEREEEERESEEEEEEEEEEEESESESESESESESESSESESESESES SEES ESSE EEE SESE / 
/* 


/* Procedure: xfer_inferred_boundary_edges_to_host(world) 
A 


f 
/* Description: this function transfers the inferred edges of 
/* the boundary polygon of a partial world to the host. 

fd 


[4ERRERREEREEEEEEEEEEEEEEEEEREEEEEEEEEEEEEEEEEESESEEERESEEEEEERERESE EES / 


void xfer_inferred_boundary_edges_to_host(PW, filename) 
Map_World *PW; 
char *filename; 


char *edgebuffer; 

char *start; 

int c, j, k; 

int count = 0; 

Map_Polygon *current_polygon; 
EDGE *current_edge; 


current_edge = PW->boundary->edge_list; 
for (k = 1; k < PW->boundary->degree; k++) 


if (current_edge->type == INFERRED) 
++count; 
current_edge = current_edge->next; 


c = 20 + (count * 35); 
edgebuffer = malloc(c); 
start = edgebuffer; 


/* Put the boundary polygon edges in the buffer */ 
current_edge = PW->boundary->edge_list; 
for (j = 0; j < PW->boundary->degree; j++) 


{ 
: (current_edge->type == INFERRED) 


print_flex(current_edge->v 1.x, edgebuffer); 
print_flex(current_edge->v 1.y, edgebuffer); 
nl_flex(edgebuffer); 
print_flex(current_edge->v2.x, edgebuffer); 
print_flex(current_edge->v2.y, edgebuffer); 
nl_flex(edgebuffer); 

nl_flex(edgebuffer); 

ee = curTent_edge->next; 

} 
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putb(\0’, edgebuffer); 
edgebuffer = : 
host_xfer(edgebuffer, filename); 
free(edgebuffer); 
} /* end xfer_real_boundary_edges_to_host() */ 


[ERSEEEEEESERESESESLSSSEESESESERESEEESESESESESSESES ES EESEEEESESESEESESEES / 
/* 


/* Procedure: host_xfer(buffer, filename) 
/* 


/* Description: this function transfers a data string from the buffer to the host. Not a 
/* user function; is called by data conversion functions such as xfer_raw_to_host. 
/* User would call the xfer_raw_to_host (or equivalent for global or segment data) 
fe to download data from the robot. 


[EREREREEAEAEEEEREREEEEEEEREEEEEEEERESESERERERESERERERESEREERESEREEESE / 


void host_xfer(buffer, filename) 
char *buffer; 
char *filename; 


i_port(HOST, 9600, 0, 0, 0); 
r_printf(‘\12\15 connect cable and keyin\’ \’”’); 
while (r_getchar() != ‘ ‘); 
putstr(“An”, HOST); 

i_port(HOST, 9600, 0, 0, 1); 
r_printf(‘\l12\15 ready for dump “‘); 
while (r_getchar() != ‘g’); 
putstr(“ytof “, HOST); 
putstr(filename, HOST); 

putstr(“ w \n”, HOST); 

while (r_getchar() != ‘ ‘); 
r_printf(““dumping “); 
putstr(buffer, HOST); 

putb(“4’, HOST); 

putb(“4’, HOST); 
r_printf(‘\7\7\7”’); 

return; 


[4 RARER RARE EEE EREREE EEE EEEEEREEE EE ERE EEER EE EEE EERE EERE EERE ERER REESE / 
i 

/* Procedure: finish_segment(n) 

/* 


/* Description: this function completes segments at the end of a data run. Necessary 


/* because the linear fitting function only terminates a segment based on the 
’ data - it has no way of knowing that the user has stopped collecting data. 


[HARARE NERA EERE REE EEE EERE EERE EEE ERE EREEEEEEE EE RRERERRERERERERER ERE / 
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void finish_segment(n) 


int n; 


{ 


LINE_SEG *seg_ptr; 
if (segment_data({n].m00 > 10.0) 
{ 


seg_ptr = end_segment(n); 
eed n); 
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APPENDIX D. ODOMETRY CORRECTION SOURCE CODE 


‘ha 

* file : nav.c 

* purpose : All robot subroutines required for navigation 
* 


*/ 
#include “mml.h” 
/* declaration of functions and return values */ 


extern void wait_pointQ; 

extern int wait_segmentl(); 

extern int wait_segment(); 

extern void get_robot_speed(); 

extern void get_s_zero(); 

extern void get_initial_position(; 
extern void report_configuration(); 
extern CONFIGURATION get_sonar_config(); 
extern void correct_odometry_error(); 
extern void enable_display_status(); 
extern void displaystatus(); 


[RERREEREEREEEREEEEEESEEEEEEEEEEEERESEEER ES EEE EEESES EELEREEESERESE SESE ES 
FUNCTION: wait_point(pt) 
PARAMETERS: POINT pt 
PURPOSE: Busy wait until the the closest point of approach to 
parameter pt, then return. 
RETURNS: void 


CALLS: get_rob0; 

COMMENTS: 16 November 92 - Dave MacPherson 
EREEEEEEEAEEEEEREEERESEEREEEEEREREREEESEREREREEEREEEEEEREEEREREEERER ELE / 
#define FLT_MAX 3.40282347e+38 
void wait_point(pt) 

POINT *pt; 


{ 
double dist = FLT_MAX; 
CONFIGURATION now; 


get_rob0(&now); 
while (dist > DIST(now.x, now.y, pt->x0, pt->y0)) 


dist = DIST(now.x, now.y, pt->x0, pt->y0); 
‘egmaanilc 
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)/* wait_point(pt) */ 


EOL ER TEER O ROLE MNES Te OEP E PET OT ST CENT TTT 
* 

FUNCTION: wait_segment1() 

PARAMETERS: none 

PURPOSE: busy wait until the currentline segment being built 

is completed or the robot travels a distance greater 

than the parameter length. 

RETURNS: integer value equal to the segment count 

CALLED BY: main 


CALLS: path. length0; 
COMMENTS: 18 November 92 - Dave Mac 


Pherson 
BRKKBKKEBKKEKKKKEKKEKEKESERESESESESKSEKESEKSESESKSKESKEKKEKEKEKSEKSEKEESESAEKESLKEEEE 
*/ 
int wait_segment1() 
int seg_count; 


double current_pos; 
double length = 100.0; 


current_pos = path_length(); 

seg_count = segment_data_log{0].count; 
r_printf(‘\12 seg_count => “) 
r_printfi(seg_count); 

while (1) 


{ 
if ((path_lengthQ - current_pos) > length) 


seg_count = -1; 
if (segment_data_log[0].count > seg_count) 


return (seg_count); 


Me He He Me He Me He ae He ae He ae He He He Me Me Me He he he he He eH He HAA KA KAKEAKAKAAAKAKAKKAAAKAKAKKAKAAAAKAAAKAKKRAKHKAKEKKEE 
* 


FUNCTION: wait_segment() 
TERS: 


COMMENTS: 11 September 92 - Dave MacPherson 


ERREEEREEEEREEEEEEEREREE ESE EEE ERERERERERERE EE EEREREEEEEEREEEEEREEEEEES / 
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f 

int wait_segment() 
{ 
int seg_count; 


seg_count = segment_data_log[0].count; 
r_printf(‘\l2 seg_count => “‘); 
T_printfi(seg_count); 

while (segment_data_log[0].count == seg_count); 
return(seg_count); 


*/ 


[REFEREREESERESEEESEEEEESEEESEESESERESESESEESESESESESESESESESSESESESE EES 


FUNCTION: get_robot_speed() 

PARAMETERS: none 

PURPOSE: sets the robot’s speed for the entire mission 
based upon user input 

RETURNS: void 

CALLED BY: user(Q) 

CALLS: NONE 

COMMENTS: 12 September 92 - Dave MacPherson 


REMAKE A EAKREEAERREKERERERERRREEERERERESEREREEEEEEEEEREREEEEEEEEEE EEE ES / 


void get_robot_speed() 
{ 
double sp; 


r_printf(‘\l2 Enter desired robot speed. “‘); 
sp = getreal(CONSOLE); 
peenerh 


[BERRA EAEEEEEEEERERERREEE EEE EERE E EEE EEE EER EREEEEEEEEEEEEREEE EEE 
FUNCTION: get_s_zero() 
PARAMETERS: none 
PURPOSE: sets the robot’s s_zero for the entire mission 
based upon user input 
RETURNS: void 
CALLED BY: userQ) 
CALLS: NONE 
COMMENTS: 12 September 92 - Dave MacPherson 


RESEREERESEEESELERESEELERERAEESESRALES EEA SLE SESSESSELESSEEEERESEEERERERER / 
void get_s_zero() 


double s_zero; 
r_printf(‘“\12 Enter desired s_zero “‘); 


$_zero = getreal(CONSOLE); 
size_const(s_zero); 
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[PRSSSEEEEEESSSES EO ESEEEEEESSSESESESESESESESESESERESESESEESESESESESESESE 
FUNCTION: get_initial_position() 
PARAMETERS: none 
PURPOSE: get the initial robot configuration 
based upon user input 
RETURNS: void 
CALLED BY: userQ 
CALLS: NONE 
COMMENTS: 29 Oct 92 - Dave MacPherson 


SARERERERAAEERESEKERERESESEEEEERESESLESEEEESESESESESEEEERELESESESERESSES / 
void get_initial_position() 


double x; 
double y; 
double t, 
double k; 
CONFIGURATION p; 


r_printf(‘\l2 Enter the starting x position: “‘); 
x = getreal(CONSOLE); 

r_printf(‘\12 Enter the starting y position: “‘); 
y = getreal(CONSOLE); 

r_printf(‘\12 Enter the starting orientation: “‘); 
t = getreal(CONSOLE); 

r_printf(‘\l2 Enter the starting kappa: “); 

k = getreal(CONSOLE); 

oe eee y, t, k, &p)); 


[BERR RRRAAERE EERE AREER EE REK ERA EERE BREE EERE EERE SERRA ERE ERERERERE 


FUNCTION: report_configuration() 
PARAMETERS: none 

PURPOSE: gets the current robot configuration 
and then displays it to the screen 

RETURNS: void 

CALLED BY: userQ) 

CALLS: NONE 

COMMENTS: 29 Oct 92 - Dave MacPherson 


BEERKEERKEREAEEREEE EERE EE EEEEEEEEREREREREEE ERE REE EEEEE BEERS EREREEEES EE / 
void report_configuration() 


r_printf(‘\l2 Current Robot Config: x =>”); 
r_printfr(vehicle.x, 2); 

r_printf(“ y =>”); 

r_printfr(vehicle.y, 2); 

r_printf(‘‘ theta =>”); 
r_printfr(r2d(vehicle.t), 2); 

r_printf(‘\12”); 

} 
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[REEEEEERESESEEREEEEEEEEEEEEEEEEEESEEEEREVEDESESSEEESESESESEEEESSESESESESES 
FUNCTION: report_pathO 
PARAMETERS: none 
PURPOSE: gets the current robot path 
and then displays it to the screen 
RETURNS: void 


COMMENTS: 17 May 93 - Dave MacPherso 


sseseSsRCAeSESESSSAECESSRSANSONSEORFEEEEANSHEAESARSASSEERESESEESEEEEE, 


void report_path() 


r_printf(‘\12 Robot current path: x =>”); 
r _printfr(current_robot_path.pc.x, 2); 
r_printf(“ y =>”); 
r_printfr(current_robot_path.pc.y, 2); 
r_printf(“ theta =>”); 
r_printfr(r2d(current_robot_path.pc.t), 2); 
r_printf(“ kappa =>”); 
r_printfr(current_robot_path.pc.k, 2); 
oo 2”); 


[ERRRRERREEREEREEEREEREREEEEREREE EAE REESE REE EEE EERE EREEE ERE HERE EE EEEERE 


FUNCTION: get_sonar_config() 
PARAMETERS: 


COMMENTS: 11 September 92 - Dave MacPherso 

SAEKREESASEEEEES ERASE EEESREA ES EESE SEES EESAAE ERIE SEES EEERS EEE EEEEEEE/ 
CONFIGURATION get_sonar_config(seg_count) 

int seg_count; 


{ 
CONFIGURATION Qsonar; 


Qsonar.x = segment_data_log[0].array[seg_count].tailx; 
Qsonar.y = segment_data_log[0].array[seg_count].taily; 
Qsonar.t = atan2(segment_data_log[0].array[seg_count].heady - 
segment_data_log(0].array[seg_count].taily, 
segment_data_log[0].array(seg_count].headx - 
segment_data_log{0].array[seg_count].tailx); 

Qsonar.k = 0.0; 

ian Qsonar; 


[FERERREREREAAEEEEREREEEEEEEREREEEREEEREAEREEEEEEREEEREREREEEEREREESERES 


FUNCTION: correct_odometry_error(Qsonar, Qmodel) 
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Oe ee NE ee ee 


PARAMETERS: 

PURPOSE: 

RETURNS: 

CALLED BY: 

CALLS: NONE 

COMMENTS: 11 September 92 - Dave MacPherson 
SRELESSSSESESEESSESESESEESESESELESESESEREREEREEEEESESESESLERESESESESEEESS / 
void correct_odometry_error(Qsonar, Qmodel) 

CONFIGURATION Qsonar, Qmodel; 


{ 

CONFIGURATION Qodo_inv; 
CONFIGURATION E, Qact, Qodo; 
CONFIGURATION Qact._ inv, X1, x1 _inv; 


get_rob0(&Qodo); 

inverse(&Qodo, &Qodo_inv); 
comp(&Qodo_inv, &Qsonar, &X1); 
inverse(&X1, &X1 ~inv); 
comp(&Qmodel, &X1_inv, &Qact); 
set_rob(&Qact); 


inverse(&Qact, &Qact_inv); 
comp(&Qodo, &Qact_inv, &E); 
wait_timer(100); 

r_printf(‘\l2 E =>“); 

r _printfr(E. X, 3); 





r_printfr(E. t, 3); 


wait_timer(100); 

r_printf(‘\12 Qsonar => “‘); 

r_printfr(Qsonar.x, 3); 

r_pnintf(“ “); 

t —printfr(Qsonar.y, 3); 
r_printf(“ os 

r “ printfr(Qsonar.t, 3); 


wait_timer(100); 
r_printf(‘\12 Qodo => “); 
eae x, 3); 


—P 
rprintfr(Qodo. t, 3); 


wait_timer(100); 
T_printf(‘\12 Qact => “); 
r_printfr(Qact.x, 3); 
r_printf(“ i 
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r_printfr(Qact.y, 3); 
rt _printf(‘“ “); 
r_printfr(Qact.t, 3); 


} 


[#ERREEREEEEREEEREREREREEEESEEE SES ERESEREESEEEEEEESEEEEEESESEEESEEEEE ES / 


/* Procedure: displaystatus 
/* 


/* Description: called every 10 ms, this routine provides an update of 
/* the current status to the lap-top as an aid in debugging 
/* level 4 problems. 

* 


[BERR REEEREEEEEEEEREEEREEEEEEREREEESEREREEEEEEEEEEESEEEEEEEEEEEEE EERE / 


void displaystatusQ) 
{ 


if (status != cur_display_status) 


r_printf(‘\n\nCurrent status is “); 
switch (status) 


{ 

case SSTOP: 

r_printf(“SSTOP\n”); 

break; 

case SLINE: 

r_printf(“SLINE\n”); 

break; 

case SBLINE: 

r_printf(“SBLINE\n”); 

break; 

case SFLINE: 

r_printf(“SFLINE\n”); 

break; 

case SCONFIG: 

ee Onn 
Teak, 

case RMOVE: 

r_printf(“RMOVE\n”); 

break; 

case SERROR: 

r_printf(“SERROR\n”); 

break; 

default: 

t_printf(““UNKNOWN\n”); 

break; 


}/* end switch */ 
}/* end if */ 
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cur_display_status = status; 
‘ks 
if (i_am_here != last_i_am_here) 


r_printf(‘\nI_am_here => “‘); 
t_printfi(i_am_here); 
last_i_am_here = i_am_here; 
‘ 


} 


[BERRRREE EAHA REE ER EEE EE EER ERER EERE ERE EE EEREREREEERERERELEEREREES / 
/* Procedure: enable_display_status() 
i 





/* Description: Lowers interrupt mask to allow level 1 interrupts 
ie 


[HH RA A A RAR AAR HR A AT NR HA A AT NOR RRR A TAH A RAAT A AC AH A A TE A A HEH / 


void enable_display_status() 


i_imaskdisplaystatus(); 
} 





APPENDIX E. CARTOGRAPHY SOURCE CODE 


HEKKEAKHBKERESKHKHKKEKKKKHESKKKSEKESSESEKEKEERSESKEKEKEKESECSESEKEKESESEKKEKEKSEE 


FILENAME: map_world.c 

PURPOSE: test file for simulating automated cartography 
CONTAINS: 

LAST UPDATE:10 July 93 


REEEREEEEERE EE ERERREEEEEREEEEEREEREEEEEEREREESESEEEEEESEREREREEEEEEEEES / 


#include “mml.h” 
#include “cartography.h” 
#include “spatial.h” 


[BEREREREAEEEEEREREEEE REE EREEEREEEEEEEE EEE REESE EE ERE EEERE REESE REREEES EEK 


FUNCTION: add_hole_to_world() 
PURPOSE: Adds a hole polygon to an existing 
Map World. 
RETURNS: void 
CALLED BY: ANYBODY 
CALLS: 
COMMENTS: The hole polygon can only be added after the 
boundary polygon has been added to the Map World. 
HHH A GAA RE RARE EAA HH EH RRA HAHA EERE ERE R ARE AK EERE KEE EEE EERE EEE 
void add_hole_to_world(H, W) 
Map_Polygon *H; 
Map_World *W; 
{ 


Map_Polygon *current_polygon; 
int 1; 


(W->boundary == NULL) 


t_printf(“Error: the boundary polygon must be added first.””); 
/* exit(0); */ 


} 
if (W->hole_list == NULL) 


W->hole_list = H; 
r_printf(‘\l2The first hole was added to the partial map.”); 
} 


else 


{ 
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if((w = (Map_ World si il World))) == NULL) { 
/* fatal(“create_world: malloc\n”); * 
‘ exit(0); */ 


/* initialize fields */ 
w->boundary = NULL; 
w->hole_list = NULL; 
w->degree = 0; 
r_printf(‘\n Created a new partial world.”); 
return(w); 
} /* create_map_world */ 
[REE REREEEREREEEREEEEREREEESEEEREEEREEREEEEEEEEEEEESEEEREREEEERESEEERESESE 
FUNCTION: make_edge() 
PURPOSE: creates a new edge 
RETURNS: EDGE 
CALLED BY: ANYBODY 


CALLS: 
COMMENTS: This function builds a new edge. 


BURKE EA KEE RERE EEE EREERE EEE EEE EERE EK ERE EEE EERE EERE EEE EE ERE EE EREREEEE AREY / 
EDGE *make_edge(x1, y1, x2, y2, type) 
double x1, y1, x2, y2; 
int type; 
{ 
EDGE *el; 
if ((¢1 = (EDGE *)malloc(sizeof(EDGE))) == NULL) 


r_printf(“Error make_edge: malloc.\n”); 





/* exit(0); */ 
el->vL.x=x1; 
el->vl.y =yl; 
el->v2.x = x2; 
el->v2.y = y2; 
el->type = type; 
return el; 

} /* end make edge */ 


[A HMA A HH HH A HH HR Ae He He Be ee He Bh he eH ee he Ae He A He ee AI I He He He ek A A BK IC 


FUNCTION: complete() 

PURPOSE: Evaluates a partial world to see if it is 

complete. 

RETURNS: int 0 = FALSE, 1 = TRUE. 

CALLED BY: ANYBODY 

CALLS: poly_complete 

COMMENTS: Uses the poly_complete function to evaluate the 
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completeness of each component polygon in the partial world. 
RHEKESSEKEKKKEKRKESSKSESKESESSKSSESSSEKESEKECEKEKKSESSSSESESSESEKESSEEKESEEKSEEESESES 


eeeS4/ 

int complete(w) 
Map_World *w; 
{ 


Map_Polygon *current_polygon; 
int i; 
int count = 0; 


if(w->boundary == NULL && w->hole_list == NULL && 
oes == () 


r_printf(““The world is not complete.\n”); 
return 0; 


} 
else if(w->degree == 1 && poly_complete(w->boundary)) 


r_printf(‘““The world is complete.\n”); 
count = 1; 


} 
else if(w->degree > 1 && poly_complete(w->boundary)) 
{ 


current_polygon = w->hole_list; 
if (poly_complete(current_polygon)) 
{ 


count = 2; 
r_printf(“The boundary of the world is complete.\n”); 
for (i=2; i < w->degree; i++) 


current_polygon = current_polygon->next; 
if (poly_complete(current_polygon)) 


r_printf(“The hole is complete.\n”); 
++count; 


} 
} 
} 


if (count == w->degree) 
return 1; 

else 

return 0; 


} * complete() */ 


[RARE AAA AA ARH RAHA AAA HA EK RARER ER ER EEE ERE EN EER ERE ER ERE ER ERE REE EE 


FUNCTION: ploy_complete() 
PURPOSE: Evaluates a map polygon to see if it is 
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lete. 
RNS: int 0 = oe 1 = TRUE. 
CALLED BY: ANYBODY 


t_printf(““Boundary polygon.\n”); 
print_polygon(W->boundary); 
current_polygon = W->hole_list; 
r_printf(““Hole Polygon\n”); 
print_polygon(current_polygon); 
for (i=2; 1 < W->degree; i++) 

{ 


current_polygon = current_polygon->next; 
r_printf(‘““Hole Polygon\n”); 
ia ie tas polygon); 


} 
} * end print world */ 


[9ERRREERAEE AE EEEREREREEEARE EEE EERE EEE EA EER EAE AER EREESEREEEREEEEEEERE 
FUNCTION: print_polygon() 
PURPOSE: Print all edges of a map polygon 
RETURNS: void 
CALLED BY: ANYBODY 
CALLS: 
COMMENTS: this function prints a polygon to the screen 
i A aaa ca | 
void print_polygon(p) 
Map_Polygon *p; 
{ 


EDGE *current_edge; 
int i; 


current_edge = p->edge_list; 
for (i = 0; i < p->degree; i++) 
{ 


t_printf(“\nEdge =>”); 

r_printfr(current_edge->v 1.x, 3); 

1_printfr(current_edge->v1.y, 3); 

1_printfr(current_edge->v2.x, 3); 

r_printfr(current_edge->v2. Oe 3); 

: (current_edge->type == REAL) 
T_printf(‘‘ REAL\n”); 

else 

r_printf(“ INFERRED\n”); 

Current_edge = current_edge->next; 


} > end print polygon */ 
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[BEEREREREEEEEEEERESESEEESEREESESSEESESESEESESESESEEESESESESE SESE SESE 


FUNCTION: plot_worldQ 

PURPOSE: Plots all edges of a map polygon 
RETURNS: void 

CALLED BY: ANYBODY 

CALLS: 

MENTS: Plots a partial world to the screen using gnuplot. 
ERREREREEEESESERESESESERESERERESEESESERESSSESERESESESESESAERESE SSE SELES / 
void plot_world(w) 

Cee *w; 


EDGE *current_edge; 
Map_Polygon *current_polygon; 


int i, j; 
FILE “realedges, *inferrededges; 
char command{160]; 
int count = 0; 
/* 
realedges = = fopen(“‘re real’’, 99. 'w’’); 
inferrededges = fopen(‘‘inferred”,”w”); 
*/ 
for (i=1; i <= w->degree; i++) 
if (i == 1) 
current_polygon = w->boundary; 
else if (i == 2) 
current_polygon = w->hole_list; 
else if G == 


current_polygon = w->hole_list->next; 


current_edge = current_polygon->edge_list, 
for (j = 0; j < current_polygon->degree; j++) 


if (current_edge->type == REAL) 


{ 

| haa 

fprintf(realedges, “%7.2f%7.2f\n%7.2£%7.2fm\n”, 
current_edge->v 1.x, current_edge->vl.y, 
aa aa current_edge->v2.y); 


} 
else if (current_edge->type == INFERRED) 


{ 

/* 

fprintf(inferrededges, ““%7.2f%7.2fn%7.2£%7.2fn\n”, 
current_edge->v 1.x, current_edge->v1.y, 
current_edge->v2.x, current_edge->v2.y); 

of 
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} 
current_edge = current_edge->next; 
2 


Fprntrealedges, “\n”); 
te tf(inferrededges, “\n’’); 
} 


fclose(realedges); 

fclose(inferrededges); 

sprintf(command, “gnuplot %s”, “world_plot.cmd”); 
: system(command); 


} * end plot_world */ 


[HREEREAEEEREEARREREREEEEEEEEEREREEEREREEEEEREREEEREREEEREEESEEESEEES EES 


FUNCTION: plot_polygonQ 

PURPOSE: Plots all edges of a map polygon 

RETURNS: void 

CALLED BY: ANYBODY 

CALLS: 

COMMENTS: Plots a polygon to the screen using gnuplot. 


HEEREREEREEREREREREREEEEERAEREERERESEREEEREREREEEEEESEEEEEREEEERERESE S / 


void plot_polygon(p) 
Map_Polygon *p; 
{ 


notta *current_edge; 


FILE *realedges, *inferrededges; 
char command[160]; 
/* 
realedges = fopen(“real”,”w”); 
inferrededges = fopen(“inferred”,”w”); 
bd | 


current_edge = p->edge_list; 

for (i = 0; i < p->degree; i++) 

{ 

if (current_edge->type == REAL) 

h 

fprintf(realedges, “%7.2f%7.2f\n%7.2f%7.2f\n”,” 
current_edge->v 1.x, cutrent_edge->v1.y, 
current_edge->v2.x, current_edge->v2.y); 

*/ 


} 
else if (current_edge->type == INFERRED) 
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{ 
hd 
fprintf(inferrededges, “%7.2f%7.2fn%7.2f%7.2fn”, 


current_edge->v 1.x, current_edge->v1.y, 
ee current_edge->v2.y); 


} 
‘aeatidl = current_edge->next, 


i 
fclose(realedges); 
fclose(inferrededges); 
sprintf(command, “gnuplot %s”, “polygon_plot.cmd”); 
system(command); 
* 
} /* end plot_polygon */ 


[RAEREREREREREREELERESERESEERESEEESEEEEREEEESEEEEEEREREEESEEEEEEEESES ES 
FUNCTION: add_edge_to_polygonQ 
PURPOSE: Adds a new edge to a map polygon 
RETURNS: Map Polygon * 
CALLED BY: ANYBODY 
CALLS: 
COMMENTS: this function allocates space for an edge and 

adds it to a polygon 


€EGKAEEEAAE EAE EEEAEAAEGEAEEEEAEEEADA EAE EEEEREEEREREES EAA SEAAE EE EEESEEEEE/ 


void add_edge_to_polygon(new_edge, p) 
EDGE *new_edge; 
Map_Polygon *p; 
{ 
EDGE *current_edge; 
int i; 
f (p->degree == 0) 
p->edge_list = new_edge; 
} 
else 


{ 
current_edge = p->edge_list; 
for (i = 1; i < p->degree; i++) 


current_edge = current_edge->next; 


current_edge->next = new_edge; 

new_edge->previous = current_edge; 

new_edge->next = p->edge_list; /* circularly linked list */ 
oe _list->previous = new_edge; 
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++p->degree; 
} * end add_edge_to_polygon */ 


[RESSEESEESESESESERESESESESESESESEERESESSEESESESESEESESESASESESESESESESES ES 


FUNCTION: create_map_polygon() 

PURPOSE: create instance of a map polygon 

RETURNS: Map Polygon * 

CALLED BY: ANYBODY 

CALLS: fatalQ <utilities.c> 

COMMENTS: this function allocates space for a map_polygon and 
retums a pointer to it. 


BRERERERERESRERERESERERESEREREEEEESESESEREEEREEEEEREEESEEEES EKER ESEREES / 
Map_Polygon *create_map_polygon() 


} 


Map_Polygon *p; 


/* allocate memory for a polygon */ 
if ((p = (Map_Polygon *) malloc(sizeof(Map_Polygon))) == NULL) 
{ 


/* 
* fatal(‘‘create_polygon: malloc\n”); exit(FAILURE); 
Es 


r_printf(‘‘malloc failed for create_map_polygon\n”); 
/* exit(0); */ 


} 

/* initialize fields */ 
p->edge_list = NULL, 
p->previous = NULL; 
p->next = NULL; 
p->degree = 0; 


r_printf(‘\n Created a map polygon.\n”); 
return (p); 
/* end create_map_polygon() */ 


[RRR RAERA AAA HE AAA ERE AKER EEA ARKH EERE KEK EKER ERE EE AKE KER EE EEK 


FUNCTION: next_scan_config() 

PURPOSE: determines the path to the closest inferred 
for the next translational scan. 

RETURNS: void 

CALLED BY: ANYBODY 

CALLS: fatal() <utilities.c> 

COMMENTS: 


HMMM AAA EAA AREER EEE EKER ERE EEEEERERERERE EEE EEE EEE KE REEEEEEEEREEEEEES YE / 
void next_scan_config(w, C) 

Map_ World *w; 

CONFIGURATION *C; 


void analyze_closest_edgeQ); 
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Map_ Polygon *current_polygon; 
EDGE “current_edge; 

int i, j; 

double centerx, centery; 

double edge_dist; 

double closest_edge_dist = 1000.0; 
EDGE *closest_edge; 


r_printf(“Entered the function next_scan_config.\n”); 
for (i = 0; i < w-> degree; i++) 


{ 

if (i = 0) 

current_polygon = w->boundary; 
else if (i == 1) 

current_polygon = w-> hole_list; 
else 


current_polygon = current_polygon->next, 


current_edge = current_polygon->edge_list; 
for (j = 0; j < current_polygon->degree; j++) 
{ 


i (current_edge->type == INFERRED) 


r_printf(‘“\nEdge =>”); 

r_printfr(current_edge->v1.x, 3); 
r_printfr(current_edge->v1.y, 3); 
r_printfr(current_edge->v2.x, 3); 
r_printfr(current_edge->v2.y, 3); 

centerx = (current_edge->v1.x + current_edge->v2.x)/2.0; 
mead = (current_edge->v1.y + current_edge->v2.y)/2.0; 


printf(“\nEdge Center => %7.2£%7.2f”, 
centerx, centery); 
bd 


edge_dist = sqrt((centerx - C->x)*(centerx - C->x) 
+ (Centery - C->y)*(centery - C->y)); 


T_printf(‘“\nedge_dist => “‘; 
r_printfr(edge_dist, 3); 


t (edge_dist < closest_edge_dist) 
closest_edge = current_edge; 
closest_edge_dist = edge_dist; 


r_printf(‘\nclosest_edge_dist =>’); 
ania ai 3) 


current_edge = current_edge->next; 


} 





} * end for loop */ 
analyze_closest_edge(closest_edge, C); 


[EESEERSESERESESEESERESESEESESESEEESESSSSSESESESESESESESEEESSE SE SESSESESE 


FUNCTION: analyze_closest_edge() 
PURPOSE: determines the path to the closest inferred 
for the next translational scan. 
RETURNS: path list 
CALLED BY: next_scan_config 
CALLS: — <utilities.c> 
1M. . 


HAEEREEEEEEEREEEEREREEEEEEESEREREREEEEEEESESEEEEERESESEREEESEREESESESS | 


void analyze_closest_edge(closest_edge, C) 
EDGE *closest_edge; 


CONFIGURATION *C; 

{ 
CONFIGURATION path], path2; 
double centerx, centery; 

/* 


printf(‘\AnThe closest edge is => %7.2£%7.2£%7.2£%7.2f\n”, 
closest_edge->v 1.x, closest_edge->v1.y, 
closest_edge->v2.x, closest_edge->v2.y); 

*/ 


centerx = (closest_edge->v 1.x + closest_edge->v2.x)/2.0; 
centery = (closest_edge->v1.y + closest_edge->v2.y)/2.0; 


/* the first backtrack path starts at the 
robot current configuration */ 
path1.x = C->x; 

pathl.y = C->y; 

path1.k = 0.0; 


/* the back track path ends at the center of the 
closest inferred segment */ 

path2.x = centerx; 

path2.y = centery; 

path2.k = 0.0; 


if (centerx < C->x) 


path1.t = 3.14; 

if (centery < C->y) 
path2.t = -1.57; 

else if (centery > C->y) 
path2.t = 1.57; 

} 


else 


{ 
path1.t = 0.0; 








if (centery < C->y) 
path2.t = -1.57; 

else if (centery > C->y) 
path2.t = 1.57; 


} 
r_printf(‘\nThe first ‘path element is => “); 


r_printfr(path 1.k, 3); 
r_printf(‘“\aThe =e path element is => “’); 
r _printfr(path2. x, 3); 


r_printfr(path2.k, 3); 
} * end analyze_closest_edge */ 
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FILENAME: mapper8.c 

PURPOSE: The Global spatial learning algorithm 
using the Free-Space-Model. 

CONTAINS: Functions for automated cartography 
AUTHOR: Dave MacPherson 

DATE: 10 July 1993 


EEEEEEEEEEESELESEEEAEEESESEEEREEEEESESESESESESESESESESESEEEESES ESE SESS / 


#include “mml.h” 
#include “cartography.h” 


extern LINE_SEG *get_current_segmentQ; 
user() 
{ 


CONFIGURATION C, first, second, third, fourth, fifth, sixth; 
Map_World *PW; 
Map_ Polygon *B, *H1, *H2; 


void initialize(); 

void find_orthogonal_orientation(); 
void follow_hallway(); 

void wall_follower(); 

void cleanup(); 

void translational_scanning(); 
void integrate(); 

void next_scan_config(); 

void turn_right(); 

void bline_turn_right(); 

void turn_leftQ; 

void turn_around(); 

void turn_around1(); 

void both_seg_correction(); 
void translational_scanning1(); 


/* Create a partial world */ 
PW =create_map_world(); 


/* Create a boundary polygon */ 
B = create_map_polygon(); 


/* Add the empty boundary polygon to the world */ 
add_boundary_to_world(B, PW); 


def_configuration(0.0, 0.0, 0.0, 0.0, &C); 
initialize(&C); 
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find_orthogoral_orientation(&first); 
iid (! complete(PW)) 


translational_scanning1(C, PW); 
oe C); 


cleanup(PW); 
) /* end user */ 


[REEREEEREEAEEEEESESEREEEESERESERESEREREEEEEEEEEEEEAERSESEESEEEEESESESESE 


FUNCTION: translational_scanning1!() 

PARAMETERS: C, PW 

PURPOSE: Executes a single translational scan for automated 
cartography. Builds a boundary polygon from the segments gathered 
by the robot. 

RETURNS: void 

CALLED BY: user 

CALLS: report_configurationQ 

COMMENTS: 11 July 93 - Dave MacPherson 

TASK: Level 0 
HRERRERERELEEEREREREREEEEEERERESEEREEEEEERESEREEEEEEEEEEDEEEEEEE EERE KE / 
void translational_scanning1(C, PW) 

CONFIGURATION C; 

Map_World *PW; 


{ 
EDGE *el, *e2, *e3; 
LINE_SEG *right_side_seg; 
LINE_SEG *left_side_seg; 
int i; 
int done = 0; 
int count; 


line(&C); 
while (sonar(FRONTL) < 9.3 Ii sonar(FRONTL) > 100.0) 
{ 


report_configuration(Q); 
wait_timer(100); 
} 


for (i = 0; i < segment_data_log[0].count; i++) 


e2 = make_edge(segment_data_log[0].array[i].headx, 
segment_data_log[0].array[i].heady, 
segment_data_log{0].array[i].tailx, 
segment_data_log[0].array[i).taily, 

REAL); 

i ait aaa PW->boundary); 
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if air aaa la != NULL) 


el = PW->boundary->edge_list->previous; 
/* get the last edge added to the boundary polygon */ 


right_side_seg = get_current_segment(7); 

e2 = make_edge(right_side_seg->headx, rights side. ria 
right_side_seg->tailx, right_side_seg->taily, RE. 

if (e1->v2.x != e2->v1.x) 


e3 = make_edge(el->v2.x, el->v2.y, e2->v1.x, e2->vl.y, INFERRED); 
aiesteas ele PW->boundary); 


add_edge_to_polygon(e2, PW->boundary); 
} * end if */ 


r_printf(‘\12 The degree of the boundary polygon is.”); 
r_printfi(PW->boundary->degree); 


} /* end translational_scanning */ 





| hashaihaibaatadiaiadteateitaitaitaatadiaala aa ataataateainataatealeaatiaiataateaaaiiaiaiaeaiaaiatiatiadiaaiatiaaiatiatiatintitiaiiatintintiatiatintintiatintintintintiadindindintintiaaiatiatiad 


FUNCTION: turn_aroundQ 

PARAMETERS: none 

PURPOSE: Rotates 180 degrees to turn the robot 
around in a narrow hallway. 

RETURNS: void 

CALLED BY: user 

CALLS: report_configuration0 

COMMENTS: 27 June 93 - Dave MacPherson 
TASK: Level 0 | 


MAM A AR HR AA HA AA HH RAR AH AA REA REA HER ERA RAE EERE EERE EE EEE EES EEE EEE & / 
void turn_around() 





r_printf(‘\12 Entered the turn around part.”); 
stop0Q); 
wait_timer(30); 
rotate(PI); 
while(vehicle.t < 3.1); 
wait_timer(100); 
report_configuration(); 
speed0(15.0); 
} * end turn_around() */ 


[RRR KEE EEA KERR A AAR REHEAT RRAH AK RER AA E EEE HAA AE EKER 
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FUNCTION: find_orthogonal_orientation 
PARAMETERS: ps 

PURPOSE: Rotates 360 degrees to obtain the best 
surface for automated cartography 

RETURNS: void 

CALLED BY: user 

CALLS: 

COMMENTS: 27 May 93 - Dave MacPherson 
TASK: Level 0 
SREKEBERESERERESEEREREREREEESEREREEREEEREEEESESEEEEEEESEEESEEEEEESESESES / 
void find_orthogonal_orientation(ps) 
Ce *ps; 


void circle_for_segments(; /* This runction command the robot to search 
for edges to extract if none are dete ced during the rotation */ 


int i; 

int seg_index; 

int seg_count, 

double seg_alpha; 

double dist; 

double seg_length; 

double seg_dist = 500.0; 

double seg_orientation; 

double headx, heady, tailx, taily; 


CONFIGURATION first; 

report_configuration(); 

qT .3); 

rotate(2*DPI); 

while (vehicle.t < 2*DPI); 

seg_count = segment_data_log[SEG_FILE].count; 

r_printf(‘\12 Got segments, count= “‘); 

T_printfi(seg_count); 

if (seg_count == 0) 

circle_for_segments(); 

/* 
* Loop through segments found, select alpha from segment that is 
* MIN_SEG_DIST to MAX_SEG_DIST cm away, and has the longest length. 
*/ 

for (i = 0; i < seg_count; i++) 


dist = segment_data_log[SEG_FILE)].array[i].r; 
seg_length = segment_data_log{SEG_FILE].array[i].length; 


/* 
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* Check the constraints for this segment. If it is better 
* than the last one, then remember it with seg_index. 
*/ 


if (fabs(dist) < MAX_SEG_DIST && fabs(dist) > MIN_SEG_DIST && 
seg_dist < fabs(dist) && seg_length > MIN_ROT_SEG) 
{ 


seg_index = i; 
seg_dist = dist; 


} 
}/* end of for loop */ 


/* Print out the segment that was chosen */ 

r_printf(‘\12 The closest segment to use is: “‘); 

r_printf(‘\l2 hx= “); 

r —printfr(segment_ data_logjSEG_FILE].array[seg_index].headx, 3); 
r_printf(“ hy =“); 

r _Printfr(segment_ data_log{SEG_FILE].array[seg_index].heady, 3); 
r _printf(“ tx =“ 

r_printft segment. data_log{SEG_FILE].array[seg_index].tailx, 3); 
r_printf(\ “) 

t_printfr(segment_ data _log[SEG_FILE].array[seg_index].taily, 3); 
r_printf(“ length 

r ~printfr(segment_ data _log([SEG_FILE].array[seg_index].length, 3); 
r_printf(“* Phi = “); 

seg_alpha = (segment_data_log[SEG_FILE].array[seg_index].alpha); 
r_printfr(r2d(seg_alpha), 2); 


headx = segment_data_log{SEG_FILE).array[seg_index].headx; 
heady = segment_data_log[ SEG_FILE].array[seg_index].heady; 
tailx = segment_data_log[SEG_FILE].array[seg_index].tailx; 
taily = segment_data_log[ SEG_FILE].array[seg_index].taily; 
seg_orientation = atan2(taily - heady, tailx - headx); 


r_printf(“‘ Seg orientation = “); 
r_printfr(r2d(seg_orientation), 2); 

if (seg_orientation < -HPI) 
seg_orientation = PI + seg_orientation; 


r_printf(‘\12 Rotation Amount = “‘); 
r_printfr(r2d(seg_orientation - norm(vehicle.t)), 2); 


rotate(seg_orientation - norm(vehicle.t)); 
wait_timer(1000); 
/* rotate to a position parallel to the closest segment */ 


def_configuration(0.0, 0.0, 0.0, 0.0, &first); 
set_rob0(&first); 

(*ps) = first; 

/* end find orthogonal orientation */ 
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FUNCTION: wall_follower 

PARAMETERS: path 

PURPOSE: follows the right hand wall in a hallway 
for automated cartography 

RETURNS: void 

eee BY: user 


CALL 
COMMENTS: 29 June 93 - Dave MacPherson 
TASK: Level 0 


SEREEREEEEEEEEEEREEEEEEEEEEEEEEEESEEEEESESEEEESESESESESEESEEEEEEESESE EE / 
void circle_for_segments() 


CONFIGURATION circle; 


r_printf(‘\12 No segments detected during rotation.”); 
r_printf(‘\12 Need to circle for segments.”); 
def_configuration(vehicle.x, vehicle.y, vehicle.t, 0.01, &circle); 
line(&circle); 

} /* end circle_for_segments */ 


[HERRERA REE EREEEREEE EERE REE EE ERE EEE ERE EERE EERE EE EE EEREREREEEEEE ES 


FUNCTION: wall_follower 

PARAMETERS: path 

PURPOSE: follows the right hand wall in a hallway 
for automated cartography 

RETURNS: void 

CALLED BY: user 

CALLS: 

COMMENTS: 29 June 93 - Dave MacPherson 
TASK: Level 0 

HAH E EAA AEE AKER EKER AR EEE EEAER EEE LEER EHH EL ERE HEE EEE EKER EEE EE EE / 
void wall_follower(path) 

CONFIGURATION path; 


{ 
LINE_SEG *right_side_seg; 
LINE_SEG *left_side_seg; 
double right_theta; 
double left_theta; 
double theta; 
CONFIGURATION Qodo, Qact; 
CONFIGURATION second, third; 
CONFIGURATION current; 
double right_seg_range; 
double left_seg_range; 
double obstacle_range; 
double new_x, new_y, new_t; 
int count = 0; 
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start_segment(RIGHTF); 
start_segment(LEFTF); 

line(&path); 

T_printf(\l2 Entering configuration is x => “‘); 


r_printfr(path.y, 3); 
r_printf(“ theta => “); 
r_printfr(path.t, 3); 
r_printf(“ kappa => “‘); 
r_printfr(path.k, 3); 


/* correct robot path based the right hand wall */ 
while(count < 4) /* stop after 4 turns */ 


{ 
: (sonar(FRONTL) < 120.0 && sonar(FRONTL) > 9.3) 


r_printf(‘\12 Entered the left turn part.”); 
obstacle_range = sonar(FRONTL); 


def_configuration(vehicle.x + (obstacle_range - 50.0) * cos(vehicle.t), 
vehicle.y + (obstacle_range - 50.0) * sin(vehicle.t), 

path.t + HPI, 

0.0, &path); 

line(&path),; 

start_segmeni(RIGHTF); 

while (fabs(path.t - vehicle.t) > 0.01); 

++count; 


else if (get_current_segment(RIGHTF) != NULL 
&& get_current_segment(RIGHTF)->length > MIN_WALL_SEG 
es sonar(RIGHTF) > 9.3) 


right_side_seg = get_current_segment(RIGHTF); 
right_seg_range = sonar(RIGHTF); 


right_side_seg = get_current_segment(RIGHTF); 
r_printf(‘\12 Right side line segment length =”); 
r_printfr(right_side_seg->length, 2); 
right_seg_range = sonar(RIGHTF); 

r_printf(‘\l2 Right side line segment range =”); 
r_printfr(right_seg_range, 2); 


right_theta = atan2(right_side_seg->taily - 
right_side_seg->heady, 

right_side_seg->tailx - right_side _seg->headx); 
r_printf(‘\12 Right side line segment orientation =”); 
r_printfr(right_theta, 2); 


get_rob0(&Qodo); 
tabetnacm(padh t)- 0.0) < 0.1) 
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new_x = Qodo.x; 

new_y = Qodo.y + (right_seg_range - WALL_DISTANCE),; 
new_t = Qodo.t - right_theta; 

r_printf(‘\l2 theta = 0 Correction.”); 


} 
rea if(fabs(norm(path.t) - HPI) < 0.1) 


new_x = Qodo.x - (right_seg_range - WALL_DISTANCE); 
new_y = Qodo.y; 

new_t = Qodo.t - norm(right_theta - Qodo.t); 

r_printf(‘\12 theta = HPI Correction.”); 


} 
else if(fabs(norm(path.t) + HPI) < 0.1) 


new_x = Qodo.x + (right_seg_range - WALL_DISTANCE); 
new_y = Qodo.y; 

new_t = path.t - norm(right_theta - path.t); 

r_printf(‘\12 theta = minus HPI Correction.”); 


} 
else if(fabs(norm(path.t) + PI) < 0.1 Il 
caer - PD <0.1) 


new_x = Qodo.x; 

new_y = Qodo.y - (right_seg_range - WALL_DISTANCE),; 
if (right_theta < 0.0) 

new_t = -right_theta; 

else 

new_t = Qodo.t; 

r_printf(‘\l2 theta = PI Correction.”); 

} 


def_configuration(new_x, new_y, new_t, 0.0, &Qact); 


set_rob0(&Qact); 

r_printf(‘\12 Right Wall Correction => x =“); 
t_printfr(new_x, 3); 

r_printf(“ y pe *); 

r_printfr(new_y, 3); 

r_printf(“ t= “); 

r_printfr(new_t, 3); 

if(fabs(norm(path.t) - 0.0) < 0.1 il 

fabs(norm(path.t) + PI) < 0.1 Il fabs(norm(path.t) - PI) < 0.1) 


{ 
while(fabs(path.y - vehicle.y) > 0.5); 
wait_timer(100); 


else 
wait_timer(WAIT); 
report_configuration(); 
} * end else if */ 
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rai if (sonar(RIGHTF) < 0.0) 


start_segment(RIGHTF); 
pais aaa elias 
report_configuration(); 
wait_timer(100); 
} * end while */ 

} * wall_follower */ 


[EREREERRERERERERERESESEEESEEEEEERESEESEEEEEESERESEEESESESESESESRESESESESS 


FUNCTION: follow_hallway 
PARAMETERS: ps 

PURPOSE: follows the Sp fifth floor hallway 
for automated cartography 

RETURNS: void 

CALLED BY: user 

CALLS: 

COMMENTS: 27 May 93 - Dave MacPherson 
TASK: Level 0 

BEEBE REEEREEEREREE EEE ER EREREREREREEEREREEEEEEREEEREEEEEEEESEREREEEES ES / 
void follow_hallway(ps) 

CONFIGURATION *ps; 


{ 
LINE_SEG *right_side_seg; 
LINE_SEG *left_side_seg; 
double right_theta; 
double left_theta; 
double theta; 
CONFIGURATION second, third; 
double right_seg_range; 
double left_seg_range; 

/* void both_seg_correctionQ); */ 


start_segment(RIGHTF); 
start_segment(LEFTF); 
line(&ps); 

r_printf(“\12 Entering configuration is x => “); 
r_printfr(ps->x, 3); 
r_printf(“ y => ae 
r_printfr(ps->y, 3); 
r_printf 66 theta => **); 
r_printfr(ps->t, 3); 
r_printf(“ kappa => “); 
r_printfr(ps->k, 3); 


/* correct robot path once based upon hallway walls */ 
while((sonar(FRONTL) < 9.3 ll sonar(FRONTL) > 100.0) && 


tha 
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*/ 


(sonar(FRONTR) < 9.3 !! sonar(FRONTR) > 100.0)) 
ea cence < 9.3 Il sonar(FRONTL) > 150.0) 


if (get_current_segment(RIGHTF) != NULL 
&& get_current_segment(LEFTF) != NULL) 


right_side_seg = get_current_segment(RIGHTF); 
right_seg_range = sonar(RIGHTF); 
left_side_seg = get_current_segment(LEFTF); 
left_seg_range = sonar(LEFTF); 


if (sonar(RIGHTF) > 9.3 && sonar(LEFTF) > 9.3 && 
right_side_seg->length > MIN_WALL_SEG && 
left_side_seg->length > MIN_WALL_SEG) 

{ 


both_seg_correction(right_side_seg, right_seg_range, 
left_side_ ce left_seg_range); 

} /* end if */ 

else if (get_current_segment(RIGHTF) != NULL && 
right_side_seg->length > MIN_WALL_SEG) 


right_side_seg = get_current_segment(RIGHTF); 
r_printf(‘\l2 Right side line segment length =”); 
r_printfr(right_side_seg->length, 2); 
right_seg_range = sonar(RIGHTF); 

r_printf(‘\l2 Right side line segment range =”); 
r_printfr(right_seg_range, 2); 


74 (sonar(RIGHTF) > 9.3) 


right_theta = atan2(right_side_seg->taily - 
right_side_seg->heady, 

right_side_seg->tailx - right_side_seg->headx); 
r_printf(‘\l2 Right side line segment orientation =”); 
r_printfr(right_theta, 2); 


get_rob0(&second); 
def_configuration(second.x, 
right_seg_range - 100.0, 
-right_theta, 0.0 , &third); 


set_rob0(&third); 

r_printf(‘\l2 Right Wall Correction => “); 
r_printfr(right_theta, 3); 
wait_timer(WAIT); 
report_configuration(); 

} /* end inner if */ 

else 


{ 
start_segment(RIGHTF); 
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a) 





} 
} * end else if */ 


else if (get_current_segment(LEFTF) != NULL && 
left_side_seg->length > MIN_WALL_SEG) 


left_side_seg = get_current_segment(LEFTF); 
r_printf(‘\12 Left side line segment length =”); 
r_printfr(left_side _seg->length, 2); 
left_seg_range = sonar(LE 

r_printf(‘\l2 Left side line segment Tange =”); 
r_printfr(left_seg_range, 2); 


7 (sonar(LEFTF) > 9.3) 


get_rob0(&p2); 

left_theta = atan2(left_side_seg->taily - left_side_seg->heady, 
left_side_seg->tailx - left_side_seg->headx); 

r_printf(‘\12 Left side line segment orientation =”); 
r_printfr(left_theta, 2); 


def_configuration(p2.x, 
223.0 - left_seg_range, 
-left_theta, 0.0 , &p2); 


set_rob0(&p2); 

r_printf(‘\l2 Left Wall Correction => “); 
r_printfr(left_theta, 3); 
wait_timer(WAIT); 

Nica 


else 
start_segment(LEFTF), 
} 

} 


} 

report_configuration(); 

wait_timer(100); 

} /* end while */ 

r oo \12 Forward Looking Sonar. =”); 
printfr(sonar(FRONTL), 2); 


}- set Pillow hallway */ 


[BERREREEREREEREEEREREEEEREREESEEEEEEREEEEEREREEE RENEE REE EREREERKERE RES 
FUNCTION: both_seg_correction(); 

PARAMETERS: right_side_seg, right_seg_range 

left_side_seg, left_seg_range 
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PURPOSE: corrects robot configuration to align to 
the center of the hallway 
RETURNS: void 


COMMENTS: 27 May 93 - Dave MacPherson 
TASK: Level 0 
BEREEEEEEEREEEESESESESESESESESESESERECESESESESESESESEEESESESSESESEEEE SS / 
void both_seg_correction(right_side_seg, right_seg_range, 
left_side_seg, left_seg_range) 
LINE_SEG *right_side_seg; 
double right_seg_range; 
LINE_SES *left_side_seg; 
oo left_seg_range; 


double right_theta; 
double left_theta; 
double theta; 
CONFIGURATION p2; 


r_printf(‘\l2 Use both segments for Correction.”’); 
right_theta = atan2(right_side_seg->taily - 
right_side_seg->heady, 

right_side_seg->tailx - right_side_seg->headx); 
r_printf(‘\12 Right side line segment orientation =”); 
r_printfr(right_theta, 2); 


left_theta = atan2(left_side_seg->taily - 
left_side_seg->heady, 

left_side_seg->tailx - left_side_seg->headx); 
r_printf(‘\12 Left side line segment orientation =”); 
1_printfr(left_theta, 2); 


theta = (right_theta + left_theta) / 2.0; 
get_rob0(&p2); 


def_configuration(p2.x, 
right_seg_range - 100.0, 
vehicle.t - theta, 0.0 , &p2); 


set_rob0(&p2); 
r_printf(‘\12 Both Wall Correction => “‘); 
r_printfr(theta, 3); 
while(fabs(vehicle.y) > 1.0); 

/* wait_timer(WAIT); */ 
report_configuration(); 

} /* both_seg_correction() */ 


[ARR ER AEE EEE EEE EE AE HEHEHE EER HE HEHEHE HAE HE EKA E A  E E 


FUNCTION: translational_scanning 
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PARAMETERS: ps 

PURPOSE: Scans the straight line wall segment 
surface for automated cartography 

RETURNS: void 

CALLED BY: user 


CALLS: 

COMMENTS: 27 May 93 - Dave MacPherson 

TASK: Level 0 
SHERLBEEESAEABEEESEAEREEREREESEEEREEESEEEEEEEEEESEESESEREEERESEESERESES ES / 
void translational_scanning(ps) 

CONFIGURATION ps; 


{ 


/* 


LINE_SEG *right_side_seg; 
LINE_SEG *left_side_seg; 
CONFIGURATION p2; 
intn =7; 

int m = 4; 


line(&ps); 
while conar(0) > 100.0 Il sonar(0) < MIN_SONAR_RANGE) 


/* code to steer robot down the hallway */ 
wait_timer(500); 
r_printf(‘\12 Use right side line segment for steering”’); 
right_side_seg = get_current_segment(7); 
r_printf(‘\l2 Right side line segment orientation =”); 
t_printfr(r2d(right_side_seg->alpha + HPI), > 
r_printf(\12 ea side line segment range = 
r_printfr(sonar(7), 2 
ee configuration(); 

if (fabs(right_side_seg->alpha + HPI - ps.t) < 0.2) 


{ 

get_rob0(&p2); 

def_configuration(p2.x, p2.y, right_side_seg->alpha + HPI, 0.0, &p2); 
skip(); 

line(&p2); 

r_printf(‘\12 Applying a correction using right wall.”’); 
wait_timer(500); 


r_printf(‘\12 Left side line segment orientation =”); 
left_side_seg = get_current_segment(4); 


z_printir(r2d(left_side_seg->alpha), 2); 


} 


r_printf(‘\l2Detected obstactle less than 100 cm ahead”); 
report_configurationQ; 
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if (sonar(m) < MIN_SONAR_RANGE && sonar(n) < MIN_SONAR_RANGE) 


r_printf(‘\12 Under range on both side sensors”); 

turn_right(); 

} else if (sonar(m) < MIN_SONAR_RANGE && sonar(n) > MIN_SONAR_- 
RANGE) 

turn_left(); 

else if (sonar(m) > MIN_SONAR_RANGE && sonar(n) > MIN_SONAR_- 
RANGE) 


if (sonar(m) > sonar(n)) 
turn_left(); 

else 

tum_right(); 


if (sonar(n) < MIN_SONAR_RANGE && sonar(m) > MIN_SONAR_RANGE) 
turn_right(); 
} /* end scan */ 


[BERRA RARE EER ERE EEE EERE EE EEE EEE EE EE EERE EEEEEEEEEEEEEEREE EEE EEE EE 


FUNCTION: turn_right() 

PARAMETERS: none 

PURPOSE: Turns the robot right 50.0 cm from its current 
configuration. 

RETURNS: void 

CALLED BY: user 

CALLS: 

COMMENTS: 27 June 93 - Dave MacPherson 

TASK: Level 0 


BEEK EEEEERAA EEE REE EAE RE ERA REA H AEA R RARER E EERE HEE EE ARE EEE EE EE | 
void turn_right() 


CONFIGURATION second; 


r_printf(‘\l2 Entered the turn right function”); 
get_rob0(&second); 

def_configuration(second.x + 50.0 * cos(second.t), 
second.y + 50.0 * sin(second.t), 

second.t - HPI, 0.0, &second); 

line(&second); 

while (vehicle.t > second.t); 

} /* end turn_right() */ 


[EERE EA MEERA AER EEE RE HAE E EERE ERE ARE EERE EE EREEEEREREEERER EERE 
FUNCTION: bline_turn_right() 

PARAMETERS: none 

PURPOSE: Turns the robot right using a bline function. 

RETURNS: void 
CALLED BY: user 








CALLS: none 
COMMENTS: 27 June 93 - Dave MacPherson 
TASK: Level 0 


HERERESESEEEEEREESESESESESESEESESEEESESESSSESESESESEEESESESE SESE SESESE SE / 


void bline_turn_right() 
{ 
CONFIGURATION second; 


r_printf(‘\12 Entered the bline turn right function”); 
get_rob0(&second); 
def_configuration(second.x + 50.0 * cos(second.t), 
second.y + 50.0 * sin(second.t) - 75.0, 
second.t - HPI, 0.0, &second); 
bline(&second); 
while (vehicle.t > second.t); 
} /* end bline_turn_right() */ 





[BEER RERAERAE EEA EER EERE E REESE ERE EEE E EREERESEKEEEEEE ERE EERE EES EEEEES 


FUNCTION: turn_left() 

PARAMETERS: none 

PURPOSE: Turns the robot left 50.0 cm from its current 
configuration. 

RETURNS: void 

CALLED BY: user 

CALLS: none 

COMMENTS: 27 June 93 - Dave MacPherson 

TASK: Level 0 


RRA A AREA EERE RGR ER EAE EAHA EERE EE AREER EEE EE ERE EERE AEE E ES / 
void turn_left() 


CONFIGURATION second; 


r_printf(‘\12 Entered the turn left function”’); 
get_rob0(&second); 
def_configuration(second.x + 50.0 * cos(second.t), 
second.y + 50.0 * sin(second.t), 
second.t + HPI, 0.0, &second); 
line(&second); 
while (vehicle.t < second.t); 
} /* end turn_left() */ 


[RAN A AAA HH HAA HHH A HHH HHH HH HI ICR IIE I TIA I I I TI I I I A IE IC HH I 
FUNCTION: initialize() 

PARAMETERS: CONFIGURATION ps 

PURPOSE: Starts the location trace function, 

enables all appropriate sonars, 

gets the robot’s initail speed from the user 

sets up all sonar logging and linear fitting functions. 
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RETURNS: void 

CALLED BY: user 

CALLS: 

COMMENTS: 27 June 93 - Dave MacPherson 

TASK: Level 0 
SEREREBEESEESAGEESRESESAEESSSEEEEESEESEELERESESESREEESESESEESESESESEESRES / 
void initialize(first) 

CONFIGURATION “first; 


{ 
double s = 10.0; 


buffer_loc = index_loc = malloc(300000); 
bufloc = indxloc = (double *) malloc(60000); 
loc_tron(2, 0x3f, 30); 
enable_sonar(FRONTL); 
enable_sonar(BACKL); 
enable_sonar(BACKR); 
enable_sonar(FRONTR); 


- get_robot_speed(); */ 
speed(15.0); 
size_const(s); 

/* get_initial_position(); */ 
set_rob(&first); 
r_printf(‘\l2 In the initializeQ) function.”); 
report_configuration(); 
enable_sonar(LEFTF); 
set_log_interval(LEFTF, 1); 
enable_sonar(RIGHTF); 
set_log_interval(RIGHTF, 1); 
enable_linear_fitting(LEFTF); 
enable_linear_fitting(RIGHTF) 
enable_data_logging(LEFTF, 2) SEG_FILE); 
enable_data_logging(RIGHTF, 2, 1); 


} /* end initialize) */ 


[RRRERERERRERESEEEREREREEEEEREEEREERESERESEERERERESEEEE LEER EEEEE EEE EE EES 


FUNCTION: cleanupQ 

PARAMETERS: none 

PURPOSE: Disables all sonars 

Ends all segments 

Disables data logging 

Ends the location trace function 

Turns off the robot wheel motors 

Tranfers line segment data back to the host computer 
Transfers robot trajectory data back to the host computer 
RETURNS: void 

CALLED BY: user 

CALLS: 
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COMMENTS: 27 June 93 - Dave MacPherson 

TASK: Level 0 
BREERESESEREEEEEEEEEEEEEEEESESEEEEEEEEEESESEEREEESESESEEESESECEESE SES ES / 
void cleanup(PW) 

Map_ World *PW; 

{ 


r_printf(‘\12 Performing the cleanup function”); 
disable_sonar(LEFTF); 
disable_sonar(RIGHTF); 
finish_segment(LEFTF); 
finish_segment(RIGHTF); 
disable_linear_fitting(LEFTF); 
disable_linear_fitting(RIGHTF); 
disable_data_logging(LEFTF, 2); 
disable_data_logging(RIGHTF, 2); 

loc_troff(); 


motor_on = NO; 
xfer_world_to_host(PW, “world.12July93”); 
xfer_segment_to_host(1, “segment7.12July93”); 
xfer_segment_to_host(0, “segment4. 12July93”); 
loc_trdump(“loc_dump. 1 2July93”’); 

} * cleanup() */ 
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